13 #include <pulse/xmalloc.h>
14 #include <pulse/i18n.h>
16 #include <pulsecore/core-error.h>
17 #include <pulsecore/namereg.h>
18 #include <pulsecore/sink.h>
19 #include <pulsecore/module.h>
20 #include <pulsecore/core-util.h>
21 #include <pulsecore/modargs.h>
22 #include <pulsecore/log.h>
23 #include <pulsecore/thread.h>
24 #include <pulsecore/thread-mq.h>
25 #include <pulsecore/rtpoll.h>
26 #include <pulsecore/sample-util.h>
27 #include <pulsecore/ltdl-helper.h>
28 #include <liboil/liboilfuncs.h>
29 #include <liboil/liboil.h>
36 #include "module-equalizer-sink-symdef.h"
38 PA_MODULE_AUTHOR("Jason Newton");
39 PA_MODULE_DESCRIPTION(_("General Purpose Equalizer"));
40 PA_MODULE_VERSION(PACKAGE_VERSION
);
41 PA_MODULE_LOAD_ONCE(FALSE
);
42 PA_MODULE_USAGE(_("sink=<sink to connect to> "));
44 #define MEMBLOCKQ_MAXLENGTH (16*1024*1024)
49 pa_sink
*sink
, *master
;
50 pa_sink_input
*sink_input
;
53 size_t fft_size
; //length (res) of fft
54 size_t window_size
;//even!
56 size_t samples_gathered
;
57 size_t n_buffered_output
;
59 float *H
;//frequency response filter (magnitude based)
60 float *W
;//windowing function (time domain)
61 float *work_buffer
,**input
,**overlap_accum
,**output_buffer
;
62 fftwf_complex
*output_window
;
63 fftwf_plan forward_plan
,inverse_plan
;
65 pa_memblockq
*memblockq
;
68 static const char* const valid_modargs
[] = {
79 uint64_t time_diff(struct timespec
*timeA_p
, struct timespec
*timeB_p
)
81 return ((timeA_p
->tv_sec
* 1000000000) + timeA_p
->tv_nsec
) -
82 ((timeB_p
->tv_sec
* 1000000000) + timeB_p
->tv_nsec
);
85 void hanning_normalized_window(float *W
,size_t window_size
){
86 //h = sqrt(2)/2 * (1+cos(t*pi)) ./ sqrt( 1+cos(t*pi).^2 )
88 for(size_t i
=0;i
<window_size
;++i
){
89 c
=cos(M_PI
*i
/(window_size
-1));
90 W
[i
]=sqrt(2.0)/2.0*(1.0+c
) / sqrt(1.0+c
*c
);
93 void hanning_window(float *W
,size_t window_size
){
94 //h=.5*(1-cos(2*pi*j/(window_size+1)), COLA for R=(M+1)/2
95 for(size_t i
=0;i
<window_size
;++i
){
96 W
[i
]=.5*(1-cos(2*M_PI
*i
/(window_size
+1)));
99 void hamming_window(float *W
,size_t window_size
){
100 //h=.54-.46*cos(2*pi*j/(window_size-1))
101 //COLA for R=(M-1)/2,(M-1)/4 etc when endpoints are divided by 2
102 //or one endpoint is zeroed
104 for(size_t i
=0;i
<window_size
;++i
){
107 W
[i
]=.54-.46*cos(2*M_PI
*m
);
112 void blackman_window(float *W
,size_t window_size
){
113 //h=.42-.5*cos(2*pi*m)+.08*cos(4*pi*m), m=(0:W-1)/(W-1)
114 //COLA for R=(M-1)/3 when M is odd and R is an integer
115 //R=M/3 when M is even and R is an integer
117 for(size_t i
=0;i
<window_size
;++i
){
120 W
[i
]=.42-.5*cos(2*M_PI
*m
)+.08*cos(4*M_PI
*m
);
125 void sin_window(float *W
,size_t window_size
){
126 //h = (cos(t*pi)+1)/2 .* float(abs(t)<1);
127 for(size_t i
=0;i
<window_size
;++i
){
128 W
[i
]=sin(M_PI
*i
/(window_size
-1));
133 void array_out(const char *name
,float *a
,size_t length
){
134 FILE *p
=fopen(name
,"w");
135 for(size_t i
=0;i
<length
;++i
){
136 fprintf(p
,"%e,",a
[i
]);
146 /* Called from I/O thread context */
147 static int sink_process_msg(pa_msgobject
*o
, int code
, void *data
, int64_t offset
, pa_memchunk
*chunk
) {
148 struct userdata
*u
= PA_SINK(o
)->userdata
;
152 case PA_SINK_MESSAGE_GET_LATENCY
: {
154 pa_sample_spec
*ss
=&u
->sink
->sample_spec
;
156 /* Get the latency of the master sink */
157 if (PA_MSGOBJECT(u
->master
)->process_msg(PA_MSGOBJECT(u
->master
), PA_SINK_MESSAGE_GET_LATENCY
, &usec
, 0, NULL
) < 0)
160 usec
+=pa_bytes_to_usec(u
->n_buffered_output
*pa_frame_size(ss
),ss
);
161 /* Add the latency internal to our sink input on top */
162 usec
+= pa_bytes_to_usec(pa_memblockq_get_length(u
->sink_input
->thread_info
.render_memblockq
), &u
->master
->sample_spec
);
163 *((pa_usec_t
*) data
) = usec
;
168 return pa_sink_process_msg(o
, code
, data
, offset
, chunk
);
172 /* Called from main context */
173 static int sink_set_state(pa_sink
*s
, pa_sink_state_t state
) {
176 pa_sink_assert_ref(s
);
177 pa_assert_se(u
= s
->userdata
);
179 if (PA_SINK_IS_LINKED(state
) &&
181 PA_SINK_INPUT_IS_LINKED(pa_sink_input_get_state(u
->sink_input
)))
183 pa_sink_input_cork(u
->sink_input
, state
== PA_SINK_SUSPENDED
);
188 /* Called from I/O thread context */
189 static void sink_request_rewind(pa_sink
*s
) {
192 pa_sink_assert_ref(s
);
193 pa_assert_se(u
= s
->userdata
);
195 /* Just hand this one over to the master sink */
196 pa_sink_input_request_rewind(u
->sink_input
, s
->thread_info
.rewind_nbytes
+ pa_memblockq_get_length(u
->memblockq
), TRUE
, FALSE
, FALSE
);
199 /* Called from I/O thread context */
200 static void sink_update_requested_latency(pa_sink
*s
) {
203 pa_sink_assert_ref(s
);
204 pa_assert_se(u
= s
->userdata
);
206 /* Just hand this one over to the master sink */
207 pa_sink_input_set_requested_latency_within_thread(
209 pa_sink_get_requested_latency_within_thread(s
));
212 /* Called from I/O thread context */
213 static int sink_input_pop_cb(pa_sink_input
*i
, size_t nbytes
, pa_memchunk
*chunk
) {
218 pa_sink_input_assert_ref(i
);
220 pa_assert_se(u
= i
->userdata
);
221 size_t fs
= pa_frame_size(&u
->sink
->sample_spec
);
222 size_t ss
=pa_sample_size(&u
->sink
->sample_spec
);
225 if (!u
->sink
|| !PA_SINK_IS_OPENED(u
->sink
->thread_info
.state
))
228 //output any buffered outputs first
229 if(u
->n_buffered_output
>0){
230 //pa_log("outputing %ld buffered samples",u->n_buffered_output);
232 size_t n_outputable
=PA_MIN(u
->n_buffered_output
,nbytes
/fs
);
233 chunk
->length
= n_outputable
*fs
;
234 chunk
->memblock
= pa_memblock_new(i
->sink
->core
->mempool
, chunk
->length
);
235 pa_memblockq_drop(u
->memblockq
, chunk
->length
);
236 dst
= (float*) pa_memblock_acquire(chunk
->memblock
);
237 for(size_t j
=0;j
<u
->channels
;++j
){
238 pa_sample_clamp(PA_SAMPLE_FLOAT32NE
, dst
+j
, fs
, u
->output_buffer
[j
], sizeof(float),n_outputable
);
239 memmove(u
->output_buffer
[j
],u
->output_buffer
[j
]+n_outputable
,(u
->n_buffered_output
-n_outputable
)*sizeof(float));
241 u
->n_buffered_output
-=n_outputable
;
242 pa_memblock_release(chunk
->memblock
);
245 pa_assert_se(u
->n_buffered_output
==0);
247 //collect the minimum number of samples
248 while(u
->samples_gathered
< (u
->window_size
-u
->overlap_size
)){
249 //render some new fragments to our memblockq
250 //size_t desired_samples=PA_MIN(u->min_input-samples_gathered,u->max_output);
251 size_t desired_samples
=PA_MIN((u
->window_size
-u
->overlap_size
)-u
->samples_gathered
,u
->max_output
);
252 while (pa_memblockq_peek(u
->memblockq
, &tchunk
) < 0) {
255 pa_sink_render(u
->sink
, desired_samples
*fs
, &nchunk
);
256 pa_memblockq_push(u
->memblockq
, &nchunk
);
257 pa_memblock_unref(nchunk
.memblock
);
259 if(tchunk
.length
/fs
!=desired_samples
){
260 pa_log("got %ld samples, asked for %ld",tchunk
.length
/fs
,desired_samples
);
262 size_t n_samples
=PA_MIN(tchunk
.length
/fs
,u
->window_size
-u
->overlap_size
-u
->samples_gathered
);
263 //TODO: figure out what to do with rest of the samples when there's too many (rare?)
264 src
= (float*) ((uint8_t*) pa_memblock_acquire(tchunk
.memblock
) + tchunk
.index
);
265 for (size_t c
=0;c
<u
->channels
;c
++) {
266 pa_sample_clamp(PA_SAMPLE_FLOAT32NE
,u
->input
[c
]+u
->overlap_size
+u
->samples_gathered
,sizeof(float), src
+c
, fs
, n_samples
);
269 u
->samples_gathered
+=n_samples
;
270 pa_memblock_release(tchunk
.memblock
);
271 pa_memblock_unref(tchunk
.memblock
);
273 //IT should be this guy if we're buffering like how its supposed to
274 //size_t n_outputable=PA_MIN(u->window_size-u->overlap_size,nbytes/fs);
275 //This one takes into account the actual data gathered but then the dsp
276 //stuff is wrong when the buffer "underruns"
277 size_t n_outputable
=PA_MIN(u
->samples_gathered
,nbytes
/fs
);
279 //debugging: tests if immediate release of freshly buffered data
280 //plays ok and prevents any other processing
282 chunk->length=n_outputable*fs;
283 chunk->memblock = pa_memblock_new(i->sink->core->mempool, chunk->length);
284 pa_memblockq_drop(u->memblockq, chunk->length);
285 dst = (float*) pa_memblock_acquire(chunk->memblock);;
286 for (size_t c=0;c<u->channels;c++) {
287 pa_sample_clamp(PA_SAMPLE_FLOAT32NE, dst+c, fs, u->input[c]+u->overlap_size, sizeof(float),n_outputable);
289 u->samples_gathered=0;
290 pa_memblock_release(chunk->memblock);
294 //pa_log("%ld dequed samples",u->samples_gathered);
297 chunk
->length
=n_outputable
*fs
;
298 chunk
->memblock
= pa_memblock_new(i
->sink
->core
->mempool
, chunk
->length
);
299 pa_memblockq_drop(u
->memblockq
, chunk
->length
);
300 dst
= (float*) pa_memblock_acquire(chunk
->memblock
);
301 //pa_sample_clamp(PA_SAMPLE_FLOAT32NE, u->input, sizeof(float), src+c, fs, samples);
302 //pa_sample_clamp(PA_SAMPLE_FLOAT32NE, dst+c,fs, u->input, sizeof(float), samples);
305 struct timespec start, end;
307 clock_gettime(CLOCK_MONOTONIC, &start);
309 //use a zero-phase sliding dft and overlap-add method
311 pa_assert_se(u
->fft_size
>=u
->window_size
);
312 //pa_assert_se(u->window_size%2==0);
313 pa_assert_se(u
->overlap_size
<u
->window_size
);
314 pa_assert_se(u
->samples_gathered
>=u
->window_size
-u
->overlap_size
);
315 size_t sample_rem
=u
->window_size
-u
->overlap_size
-n_outputable
;
316 //size_t w_mid=u->window_size/2;
317 //pa_log("hello world a");
318 for (c
=0;c
<u
->channels
;c
++) {
319 //center the data for zero phase
320 //zero-pad TODO: optimization if sure these zeros aren't overwritten
321 //memset(u->work_buffer+w_mid,0,(u->fft_size-u->window_size)*sizeof(float));
322 //memset(u->work_buffer,0,u->fft_size*sizeof(float));
324 for(size_t j=0;j<u->window_size;++j){
325 u->work_buffer[j]=u->W[j]*u->input[c][j];
326 u->work_buffer[j]=u->input[c][j];
329 //zero padd the data, don't worry about zerophase, shouldn't really matter
330 memset(u
->work_buffer
+u
->overlap_size
,0,(u
->fft_size
-u
->overlap_size
)*sizeof(float));
332 for(size_t j
=0;j
<u
->window_size
;++j
){
333 u
->work_buffer
[j
]=u
->W
[j
]*u
->input
[c
][j
];
336 //recenter for zero phase
337 for(size_t j=0;j<w_mid;++j){
338 float tmp=u->work_buffer[j];
339 u->work_buffer[j]=u->input[c][j+w_mid];
340 u->work_buffer[j+u->fft_size-w_mid]=tmp;
343 //pa_log("hello world b");
346 //window and zero phase shift
347 for(size_t j=0;j<w_mid;++j){
348 //u->work_buffer[j]=u->input[c][j+w_mid];
349 //u->work_buffer[j+u->fft_size-w_mid]=u->input[c][j];
350 u->work_buffer[j]=u->W[j+w_mid]*u->input[c][j+w_mid];
351 u->work_buffer[j+u->fft_size-w_mid]=u->W[j]*u->input[c][j];
353 //Processing is done here!
355 fftwf_execute_dft_r2c(u
->forward_plan
,u
->work_buffer
,u
->output_window
);
357 for(size_t j
=0;j
<u
->fft_size
/2+1;++j
){
358 ////identity transform (fft size)
359 //u->output_window[j][0]/=u->fft_size;
360 //u->output_window[j][1]/=u->fft_size;
361 ////identity transform (window size)
362 //u->output_window[j][0]/=u->window_size;
363 //u->output_window[j][1]/=u->window_size;
365 u
->output_window
[j
][0]*=u
->H
[j
];
366 u
->output_window
[j
][1]*=u
->H
[j
];
369 fftwf_execute_dft_c2r(u
->inverse_plan
,u
->output_window
,u
->work_buffer
);
373 for(size_t j=0;j<w_mid;++j){
374 const float tmp=u->work_buffer[j];
375 u->work_buffer[j]=u->work_buffer[j+u->fft_size-w_mid];
376 u->work_buffer[j+w_mid]=tmp;
380 //divide out fft gain (more stable here?)
381 for(size_t j=0;j<u->window_size;++j){
382 u->work_buffer[j]/=u->fft_size;
386 //debug: tests overlaping add
387 //and negates ALL PREVIOUS processing
388 //yields a perfect reconstruction if COLA is held
389 for(size_t j=0;j<u->window_size;++j){
390 u->work_buffer[j]=u->W[j]*u->input[c][j];
394 //debug: tests if basic buffering works
395 //shouldn't modify the signal AT ALL
396 for(size_t j=0;j<u->window_size;++j){
397 u->work_buffer[j]=u->input[c][j];
402 //overlap add and preserve overlap component from this window (zero phase)
403 for(size_t j=0;j<u->overlap_size;++j){
404 u->work_buffer[j]+=u->overlap_accum[c][j];
405 u->overlap_accum[c][j]=u->work_buffer[u->window_size-u->overlap_size+j];
408 //overlap add and preserve overlap component from this window (linear phase)
409 for(size_t j
=0;j
<u
->overlap_size
;++j
){
410 u
->work_buffer
[j
]+=u
->overlap_accum
[c
][j
];
411 u
->overlap_accum
[c
][j
]=u
->work_buffer
[u
->window_size
-u
->overlap_size
+j
];
414 //preseve the needed input for the next windows overlap
415 memmove(u
->input
[c
],u
->input
[c
]+u
->overlap_size
,(u
->window_size
-u
->overlap_size
)*sizeof(float));
416 //output the samples that are outputable now
417 pa_sample_clamp(PA_SAMPLE_FLOAT32NE
, dst
+c
, fs
, u
->work_buffer
, sizeof(float),n_outputable
);
418 //buffer the rest of them
419 memcpy(u
->output_buffer
[c
]+u
->n_buffered_output
,u
->work_buffer
+n_outputable
,sample_rem
*sizeof(float));
422 clock_gettime(CLOCK_MONOTONIC, &end);
423 elapsed=time_diff(&end, &start);
424 pa_log("processed: %ld, time: %ld",u->samples_gathered,elapsed);
426 u
->n_buffered_output
+=sample_rem
;
427 u
->samples_gathered
=0;
430 //pa_log("%ld samples queued",u->n_buffered_output);
432 pa_memblock_release(chunk
->memblock
);
438 /* Called from I/O thread context */
439 static void sink_input_process_rewind_cb(pa_sink_input
*i
, size_t nbytes
) {
443 pa_sink_input_assert_ref(i
);
444 pa_assert_se(u
= i
->userdata
);
446 if (!u
->sink
|| !PA_SINK_IS_OPENED(u
->sink
->thread_info
.state
))
449 if (u
->sink
->thread_info
.rewind_nbytes
> 0) {
452 max_rewrite
= nbytes
+ pa_memblockq_get_length(u
->memblockq
);
453 amount
= PA_MIN(u
->sink
->thread_info
.rewind_nbytes
, max_rewrite
);
454 u
->sink
->thread_info
.rewind_nbytes
= 0;
457 pa_memblockq_seek(u
->memblockq
, - (int64_t) amount
, PA_SEEK_RELATIVE
, TRUE
);
458 pa_log_debug("Resetting equalizer");
462 pa_sink_process_rewind(u
->sink
, amount
);
463 pa_memblockq_rewind(u
->memblockq
, nbytes
);
466 /* Called from I/O thread context */
467 static void sink_input_update_max_rewind_cb(pa_sink_input
*i
, size_t nbytes
) {
470 pa_sink_input_assert_ref(i
);
471 pa_assert_se(u
= i
->userdata
);
473 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
476 pa_memblockq_set_maxrewind(u
->memblockq
, nbytes
);
477 pa_sink_set_max_rewind_within_thread(u
->sink
, nbytes
);
480 /* Called from I/O thread context */
481 static void sink_input_update_max_request_cb(pa_sink_input
*i
, size_t nbytes
) {
484 pa_sink_input_assert_ref(i
);
485 pa_assert_se(u
= i
->userdata
);
487 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
490 pa_sink_set_max_request_within_thread(u
->sink
, nbytes
);
493 /* Called from I/O thread context */
494 static void sink_input_update_sink_latency_range_cb(pa_sink_input
*i
) {
497 pa_sink_input_assert_ref(i
);
498 pa_assert_se(u
= i
->userdata
);
500 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
503 pa_sink_set_latency_range_within_thread(u
->sink
, i
->sink
->thread_info
.min_latency
, i
->sink
->thread_info
.max_latency
);
506 /* Called from I/O thread context */
507 static void sink_input_detach_cb(pa_sink_input
*i
) {
510 pa_sink_input_assert_ref(i
);
511 pa_assert_se(u
= i
->userdata
);
513 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
516 pa_sink_detach_within_thread(u
->sink
);
517 pa_sink_set_asyncmsgq(u
->sink
, NULL
);
518 pa_sink_set_rtpoll(u
->sink
, NULL
);
521 /* Called from I/O thread context */
522 static void sink_input_attach_cb(pa_sink_input
*i
) {
525 pa_sink_input_assert_ref(i
);
526 pa_assert_se(u
= i
->userdata
);
528 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
531 pa_sink_set_asyncmsgq(u
->sink
, i
->sink
->asyncmsgq
);
532 pa_sink_set_rtpoll(u
->sink
, i
->sink
->rtpoll
);
533 pa_sink_attach_within_thread(u
->sink
);
535 pa_sink_set_latency_range_within_thread(u
->sink
, u
->master
->thread_info
.min_latency
, u
->master
->thread_info
.max_latency
);
538 /* Called from main context */
539 static void sink_input_kill_cb(pa_sink_input
*i
) {
542 pa_sink_input_assert_ref(i
);
543 pa_assert_se(u
= i
->userdata
);
545 pa_sink_unlink(u
->sink
);
546 pa_sink_input_unlink(u
->sink_input
);
548 pa_sink_unref(u
->sink
);
550 pa_sink_input_unref(u
->sink_input
);
551 u
->sink_input
= NULL
;
553 pa_module_unload_request(u
->module
, TRUE
);
556 /* Called from IO thread context */
557 static void sink_input_state_change_cb(pa_sink_input
*i
, pa_sink_input_state_t state
) {
560 pa_sink_input_assert_ref(i
);
561 pa_assert_se(u
= i
->userdata
);
563 /* If we are added for the first time, ask for a rewinding so that
564 * we are heard right-away. */
565 if (PA_SINK_INPUT_IS_LINKED(state
) &&
566 i
->thread_info
.state
== PA_SINK_INPUT_INIT
) {
567 pa_log_debug("Requesting rewind due to state change.");
568 pa_sink_input_request_rewind(i
, 0, FALSE
, TRUE
, TRUE
);
572 /* Called from main context */
573 static pa_bool_t
sink_input_may_move_to_cb(pa_sink_input
*i
, pa_sink
*dest
) {
576 pa_sink_input_assert_ref(i
);
577 pa_assert_se(u
= i
->userdata
);
579 return u
->sink
!= dest
;
582 int pa__init(pa_module
*m
) {
589 pa_sink_input_new_data sink_input_data
;
590 pa_sink_new_data sink_data
;
591 pa_bool_t
*use_default
= NULL
;
596 if (!(ma
= pa_modargs_new(m
->argument
, valid_modargs
))) {
597 pa_log("Failed to parse module arguments.");
601 if (!(master
= pa_namereg_get(m
->core
, pa_modargs_get_value(ma
, "master", NULL
), PA_NAMEREG_SINK
))) {
602 pa_log("Master sink not found");
606 ss
= master
->sample_spec
;
607 ss
.format
= PA_SAMPLE_FLOAT32
;
608 map
= master
->channel_map
;
609 if (pa_modargs_get_sample_spec_and_channel_map(ma
, &ss
, &map
, PA_CHANNEL_MAP_DEFAULT
) < 0) {
610 pa_log("Invalid sample format specification or channel map");
613 fs
=pa_frame_size(&ss
);
615 u
= pa_xnew0(struct userdata
, 1);
621 u
->sink_input
= NULL
;
622 u
->memblockq
= pa_memblockq_new(0, MEMBLOCKQ_MAXLENGTH
, 0, fs
, 1, 1, 0, NULL
);
627 u
->channels
=ss
.channels
;
628 u
->fft_size
=pow(2,ceil(log(ss
.rate
)/log(2)));
629 //u->fft_size=ss.rate;
631 pa_log("fft size: %ld",u
->fft_size
);
633 u
->overlap_size
=(u
->window_size
+1)/2;
634 //u->overlap_size=u->window_size/2;
636 u
->samples_gathered
=0;
637 u
->n_buffered_output
=0;
638 u
->max_output
=pa_frame_align(pa_mempool_block_size_max(m
->core
->mempool
), &ss
)/pa_frame_size(&ss
);
639 u
->H
=(float*) fftwf_malloc((u
->fft_size
/2+1)*sizeof(float));
640 u
->W
=(float*) fftwf_malloc((u
->window_size
)*sizeof(float));
641 u
->work_buffer
=(float*) fftwf_malloc(u
->fft_size
*sizeof(float));
642 u
->input
=(float **)malloc(sizeof(float *)*u
->channels
);
643 u
->overlap_accum
=(float **)malloc(sizeof(float *)*u
->channels
);
644 u
->output_buffer
=(float **)malloc(sizeof(float *)*u
->channels
);
645 for(size_t c
=0;c
<u
->channels
;++c
){
646 u
->input
[c
]=(float*) fftwf_malloc(u
->window_size
*sizeof(float));
647 memset(u
->input
[c
],0,u
->window_size
*sizeof(float));
648 u
->overlap_accum
[c
]=(float*) fftwf_malloc(u
->overlap_size
*sizeof(float));
649 memset(u
->overlap_accum
[c
],0,u
->overlap_size
*sizeof(float));
650 u
->output_buffer
[c
]=(float*) fftwf_malloc(u
->window_size
*sizeof(float));
652 u
->output_window
= (fftwf_complex
*) fftwf_malloc(sizeof(fftwf_complex
) * (u
->fft_size
/2+1));
653 u
->forward_plan
=fftwf_plan_dft_r2c_1d(u
->fft_size
, u
->work_buffer
, u
->output_window
, FFTW_ESTIMATE
);
654 u
->inverse_plan
=fftwf_plan_dft_c2r_1d(u
->fft_size
, u
->output_window
, u
->work_buffer
, FFTW_ESTIMATE
);
658 for(size_t j=0;j<u->window_size;++j){
662 //hanning_normalized_window(u->W,u->window_size);
663 hanning_window(u
->W
,u
->window_size
);
664 //sin_window(u->W,u->window_size);
665 array_out("/home/jason/window.txt",u
->W
,u
->window_size
);
666 //u->forward_plan=fftwf_plan_dft_r2c_1d(u->fft_size, u->input, u->output_window, FFTW_ESTIMATE);
667 //u->inverse_plan=fftwf_plan_dft_c2r_1d(u->fft_size, u->output_window, u->work_buffer, FFTW_ESTIMATE);
668 //u->forward_plan=fftwf_plan_dft_r2c_1d(u->fft_size, u->input, u->output, FFTW_MEASURE);
669 //u->inverse_plan=fftwf_plan_dft_c2r_1d(u->fft_size, u->output, u->input, FFTW_MEASURE);
670 const int freqs
[]={0,25,50,100,200,300,400,800,1500,
671 2000,3000,4000,5000,6000,7000,8000,9000,10000,11000,12000,
672 13000,14000,15000,16000,17000,18000,19000,20000,21000,22000,23000,24000,INT_MAX
};
673 const float coefficients
[]={1,1,1,1,1,1,1,1,1,1,
675 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
676 const size_t ncoefficients
=sizeof(coefficients
)/sizeof(float);
677 pa_assert_se(sizeof(freqs
)/sizeof(int)==sizeof(coefficients
)/sizeof(float));
678 float *freq_translated
=(float *) malloc(sizeof(float)*(ncoefficients
));
679 freq_translated
[0]=1;
680 //Translate the frequencies in their natural sampling rate to the new sampling rate frequencies
681 for(size_t i
=1;i
<ncoefficients
-1;++i
){
682 freq_translated
[i
]=((float)freqs
[i
]*u
->fft_size
)/ss
.rate
;
683 //pa_log("i: %ld: %d , %g",i,freqs[i],freq_translated[i]);
684 pa_assert_se(freq_translated
[i
]>=freq_translated
[i
-1]);
686 freq_translated
[ncoefficients
-1]=DBL_MAX
;
687 //Interpolate the specified frequency band values
689 for(size_t i
=1,j
=0;i
<(u
->fft_size
/2+1);++i
){
690 pa_assert_se(j
<ncoefficients
);
691 //max frequency range passed, consider the rest as one band
692 if(freq_translated
[j
+1]>=DBL_MAX
){
693 for(;i
<(u
->fft_size
/2+1);++i
){
694 u
->H
[i
]=coefficients
[j
];
698 //pa_log("i: %d, j: %d, freq: %f",i,j,freq_translated[j]);
699 //pa_log("interp: %0.4f %0.4f",freq_translated[j],freq_translated[j+1]);
700 pa_assert_se(freq_translated
[j
]<freq_translated
[j
+1]);
701 pa_assert_se(i
>=freq_translated
[j
]);
702 pa_assert_se(i
<=freq_translated
[j
+1]);
703 //bilinear-inerpolation of coefficients specified
704 float c0
=(i
-freq_translated
[j
])/(freq_translated
[j
+1]-freq_translated
[j
]);
705 pa_assert_se(c0
>=0&&c0
<=1.0);
706 u
->H
[i
]=((1.0f
-c0
)*coefficients
[j
]+c0
*coefficients
[j
+1]);
707 pa_assert_se(u
->H
[i
]>0);
708 while(i
>=floor(freq_translated
[j
+1])){
712 array_out("/home/jason/coffs.txt",u
->H
,u
->fft_size
/2+1);
713 //divide out the fft gain
714 for(int i
=0;i
<(u
->fft_size
/2+1);++i
){
715 u
->H
[i
]/=u
->fft_size
;
717 free(freq_translated
);
720 pa_sink_new_data_init(&sink_data
);
721 sink_data
.driver
= __FILE__
;
722 sink_data
.module
= m
;
723 if (!(sink_data
.name
= pa_xstrdup(pa_modargs_get_value(ma
, "sink_name", NULL
))))
724 sink_data
.name
= pa_sprintf_malloc("%s.equalizer", master
->name
);
725 sink_data
.namereg_fail
= FALSE
;
726 pa_sink_new_data_set_sample_spec(&sink_data
, &ss
);
727 pa_sink_new_data_set_channel_map(&sink_data
, &map
);
728 z
= pa_proplist_gets(master
->proplist
, PA_PROP_DEVICE_DESCRIPTION
);
729 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_DESCRIPTION
, "FFT based equalizer");
730 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_MASTER_DEVICE
, master
->name
);
731 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_CLASS
, "filter");
733 if (pa_modargs_get_proplist(ma
, "sink_properties", sink_data
.proplist
, PA_UPDATE_REPLACE
) < 0) {
734 pa_log("Invalid properties");
735 pa_sink_new_data_done(&sink_data
);
739 u
->sink
= pa_sink_new(m
->core
, &sink_data
, PA_SINK_LATENCY
|PA_SINK_DYNAMIC_LATENCY
);
740 pa_sink_new_data_done(&sink_data
);
743 pa_log("Failed to create sink.");
747 u
->sink
->parent
.process_msg
= sink_process_msg
;
748 u
->sink
->set_state
= sink_set_state
;
749 u
->sink
->update_requested_latency
= sink_update_requested_latency
;
750 u
->sink
->request_rewind
= sink_request_rewind
;
751 u
->sink
->userdata
= u
;
753 pa_sink_set_asyncmsgq(u
->sink
, master
->asyncmsgq
);
754 pa_sink_set_rtpoll(u
->sink
, master
->rtpoll
);
756 /* Create sink input */
757 pa_sink_input_new_data_init(&sink_input_data
);
758 sink_input_data
.driver
= __FILE__
;
759 sink_input_data
.module
= m
;
760 sink_input_data
.sink
= u
->master
;
761 pa_proplist_sets(sink_input_data
.proplist
, PA_PROP_MEDIA_NAME
, "Equalized Stream");
762 pa_proplist_sets(sink_input_data
.proplist
, PA_PROP_MEDIA_ROLE
, "filter");
763 pa_sink_input_new_data_set_sample_spec(&sink_input_data
, &ss
);
764 pa_sink_input_new_data_set_channel_map(&sink_input_data
, &map
);
766 pa_sink_input_new(&u
->sink_input
, m
->core
, &sink_input_data
, PA_SINK_INPUT_DONT_MOVE
);
767 pa_sink_input_new_data_done(&sink_input_data
);
772 u
->sink_input
->pop
= sink_input_pop_cb
;
773 u
->sink_input
->process_rewind
= sink_input_process_rewind_cb
;
774 u
->sink_input
->update_max_rewind
= sink_input_update_max_rewind_cb
;
775 u
->sink_input
->update_max_request
= sink_input_update_max_request_cb
;
776 u
->sink_input
->update_sink_latency_range
= sink_input_update_sink_latency_range_cb
;
777 u
->sink_input
->kill
= sink_input_kill_cb
;
778 u
->sink_input
->attach
= sink_input_attach_cb
;
779 u
->sink_input
->detach
= sink_input_detach_cb
;
780 u
->sink_input
->state_change
= sink_input_state_change_cb
;
781 u
->sink_input
->may_move_to
= sink_input_may_move_to_cb
;
782 u
->sink_input
->userdata
= u
;
784 pa_sink_put(u
->sink
);
785 pa_sink_input_put(u
->sink_input
);
789 pa_xfree(use_default
);
797 pa_xfree(use_default
);
804 int pa__get_n_used(pa_module
*m
) {
808 pa_assert_se(u
= m
->userdata
);
810 return pa_sink_linked_by(u
->sink
);
813 void pa__done(pa_module
*m
) {
818 if (!(u
= m
->userdata
))
822 pa_sink_unlink(u
->sink
);
823 pa_sink_unref(u
->sink
);
827 pa_sink_input_unlink(u
->sink_input
);
828 pa_sink_input_unref(u
->sink_input
);
832 pa_memblockq_free(u
->memblockq
);
834 fftwf_destroy_plan(u
->inverse_plan
);
835 fftwf_destroy_plan(u
->forward_plan
);
836 fftwf_free(u
->output_window
);
837 for(size_t c
=0;c
<u
->channels
;++c
){
838 fftwf_free(u
->output_buffer
[c
]);
839 fftwf_free(u
->overlap_accum
[c
]);
840 fftwf_free(u
->input
[c
]);
842 free(u
->output_buffer
);
843 free(u
->overlap_accum
);
845 fftwf_free(u
->work_buffer
);