2 This file is part of PulseAudio.
4 This module is based off Lennart Poettering's LADSPA sink and swaps out
5 LADSPA functionality for a STFT OLA based digital equalizer. All new work
6 is published under Pulseaudio's original license.
7 Copyright 2009 Jason Newton <nevion@gmail.com>
10 Copyright 2004-2008 Lennart Poettering
12 PulseAudio is free software; you can redistribute it and/or modify
13 it under the terms of the GNU Lesser General Public License as published
14 by the Free Software Foundation; either version 2.1 of the License,
15 or (at your option) any later version.
17 PulseAudio is distributed in the hope that it will be useful, but
18 WITHOUT ANY WARRANTY; without even the implied warranty of
19 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
20 General Public License for more details.
22 You should have received a copy of the GNU Lesser General Public License
23 along with PulseAudio; if not, write to the Free Software
24 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
40 #include <pulse/xmalloc.h>
41 #include <pulse/i18n.h>
43 #include <pulsecore/core-error.h>
44 #include <pulsecore/namereg.h>
45 #include <pulsecore/sink.h>
46 #include <pulsecore/module.h>
47 #include <pulsecore/core-util.h>
48 #include <pulsecore/modargs.h>
49 #include <pulsecore/log.h>
50 #include <pulsecore/thread.h>
51 #include <pulsecore/thread-mq.h>
52 #include <pulsecore/rtpoll.h>
53 #include <pulsecore/sample-util.h>
54 #include <pulsecore/ltdl-helper.h>
62 #include <xmmintrin.h>
63 #include <emmintrin.h>
68 #include "module-equalizer-sink-symdef.h"
70 PA_MODULE_AUTHOR("Jason Newton");
71 PA_MODULE_DESCRIPTION(_("General Purpose Equalizer"));
72 PA_MODULE_VERSION(PACKAGE_VERSION
);
73 PA_MODULE_LOAD_ONCE(FALSE
);
74 PA_MODULE_USAGE(_("sink=<sink to connect to> "));
76 #define MEMBLOCKQ_MAXLENGTH (16*1024*1024)
82 pa_sink
*sink
, *master
;
83 pa_sink_input
*sink_input
;
86 size_t fft_size
;//length (res) of fft
89 *effectively chooses R
91 size_t R
;/* the hop size between overlapping windows
92 * the latency of the filter, calculated from window_size
93 * based on constraints of COLA and window function
95 size_t latency
;//Really just R but made into it's own variable
96 //for twiddling with pulseaudio
97 size_t overlap_size
;//window_size-R
98 size_t samples_gathered
;
99 size_t max_output
;//max amount of samples outputable in a single
101 size_t target_samples
;
102 float *H
;//frequency response filter (magnitude based)
103 float *W
;//windowing function (time domain)
104 float *work_buffer
,**input
,**overlap_accum
,**output_buffer
;
105 fftwf_complex
*output_window
;
106 fftwf_plan forward_plan
,inverse_plan
;
109 pa_memchunk conv_buffer
;
110 pa_memblockq
*rendered_q
;
113 static const char* const valid_modargs
[] = {
124 static uint64_t time_diff(struct timespec
*timeA_p
, struct timespec
*timeB_p
);
125 static void hanning_window(float *W
,size_t window_size
);
126 static void array_out(const char *name
,float *a
,size_t length
);
127 static void process_samples(struct userdata
*u
);
128 static void input_buffer(struct userdata
*u
,pa_memchunk
*in
);
131 float * __restrict__ dst
,
132 float * __restrict__ src
,
133 float * __restrict__ overlap
,
134 const float * __restrict__ H
,
135 const float * __restrict__ W
,
136 fftwf_complex
* __restrict__ output_window
,
140 #define gettime(x) clock_gettime(CLOCK_MONOTONIC,&x)
141 #define tdiff(x,y) time_diff(&x,&y)
142 #define mround(x,y) (x%y==0?x:(x/y+1)*y)
144 uint64_t time_diff(struct timespec
*timeA_p
, struct timespec
*timeB_p
)
146 return ((timeA_p
->tv_sec
* 1000000000ULL) + timeA_p
->tv_nsec
) -
147 ((timeB_p
->tv_sec
* 1000000000ULL) + timeB_p
->tv_nsec
);
150 void hanning_window(float *W
,size_t window_size
){
151 //h=.5*(1-cos(2*pi*j/(window_size+1)), COLA for R=(M+1)/2
152 for(size_t i
=0;i
<window_size
;++i
){
153 W
[i
]=(float).5*(1-cos(2*M_PI
*i
/(window_size
+1)));
157 void array_out(const char *name
,float *a
,size_t length
){
158 FILE *p
=fopen(name
,"w");
160 pa_log("opening %s failed!",name
);
163 for(size_t i
=0;i
<length
;++i
){
164 fprintf(p
,"%e,",a
[i
]);
174 /* Called from I/O thread context */
175 static int sink_process_msg(pa_msgobject
*o
, int code
, void *data
, int64_t offset
, pa_memchunk
*chunk
) {
176 struct userdata
*u
= PA_SINK(o
)->userdata
;
180 case PA_SINK_MESSAGE_GET_LATENCY
: {
182 pa_sample_spec
*ss
=&u
->sink
->sample_spec
;
183 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
185 /* Get the latency of the master sink */
186 if (PA_MSGOBJECT(u
->master
)->process_msg(PA_MSGOBJECT(u
->master
), PA_SINK_MESSAGE_GET_LATENCY
, &usec
, 0, NULL
) < 0)
189 //usec+=pa_bytes_to_usec(u->latency*fs,ss);
190 //usec+=pa_bytes_to_usec(u->samples_gathered*fs,ss);
191 usec
+= pa_bytes_to_usec(pa_memblockq_get_length(u
->rendered_q
), ss
);
192 /* Add the latency internal to our sink input on top */
193 usec
+= pa_bytes_to_usec(pa_memblockq_get_length(u
->sink_input
->thread_info
.render_memblockq
), &u
->master
->sample_spec
);
194 *((pa_usec_t
*) data
) = usec
;
199 return pa_sink_process_msg(o
, code
, data
, offset
, chunk
);
203 /* Called from main context */
204 static int sink_set_state(pa_sink
*s
, pa_sink_state_t state
) {
207 pa_sink_assert_ref(s
);
208 pa_assert_se(u
= s
->userdata
);
210 if (PA_SINK_IS_LINKED(state
) &&
212 PA_SINK_INPUT_IS_LINKED(pa_sink_input_get_state(u
->sink_input
)))
214 pa_sink_input_cork(u
->sink_input
, state
== PA_SINK_SUSPENDED
);
219 /* Called from I/O thread context */
220 static void sink_request_rewind(pa_sink
*s
) {
223 pa_sink_assert_ref(s
);
224 pa_assert_se(u
= s
->userdata
);
226 /* Just hand this one over to the master sink */
227 pa_sink_input_request_rewind(u
->sink_input
, s
->thread_info
.rewind_nbytes
+ pa_memblockq_get_length(u
->rendered_q
), TRUE
, FALSE
, FALSE
);
230 /* Called from I/O thread context */
231 static void sink_update_requested_latency(pa_sink
*s
) {
234 pa_sink_assert_ref(s
);
235 pa_assert_se(u
= s
->userdata
);
237 /* Just hand this one over to the master sink */
238 pa_sink_input_set_requested_latency_within_thread(
240 pa_sink_get_requested_latency_within_thread(s
));
243 static void process_samples(struct userdata
*u
){
245 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
246 while(u
->samples_gathered
>=u
->R
){
248 //pa_log("iter gathered: %ld",u->samples_gathered);
249 //pa_memblockq_drop(u->rendered_q, tchunk.length);
251 tchunk
.length
=u
->R
*fs
;
252 tchunk
.memblock
=pa_memblock_new(u
->core
->mempool
,tchunk
.length
);
253 dst
=((float*)pa_memblock_acquire(tchunk
.memblock
));
254 for (size_t c
=0;c
<u
->channels
;c
++) {
264 pa_sample_clamp(PA_SAMPLE_FLOAT32NE
,dst
+c
,fs
,u
->work_buffer
,sizeof(float),u
->R
);
266 pa_memblock_release(tchunk
.memblock
);
267 pa_memblockq_push(u
->rendered_q
, &tchunk
);
268 pa_memblock_unref(tchunk
.memblock
);
269 u
->samples_gathered
-=u
->R
;
273 typedef float v4sf
__attribute__ ((__aligned__(v_size
*sizeof(float))));
274 typedef union float_vector
{
282 ////reference implementation
284 // float * __restrict__ dst,//used as a temp array too, needs to be fft_length!
285 // float * __restrict__ src,/*input data w/ overlap at start,
286 // *automatically cycled in routine
288 // float * __restrict__ overlap,//The size of the overlap
289 // const float * __restrict__ H,//The freq. magnitude scalers filter
290 // const float * __restrict__ W,//The windowing function
291 // fftwf_complex * __restrict__ output_window,//The transformed window'd src
292 // struct userdata *u){
293 // //use a linear-phase sliding STFT and overlap-add method (for each channel)
294 // //zero padd the data
295 // memset(dst+u->window_size,0,(u->fft_size-u->window_size)*sizeof(float));
297 // for(size_t j=0;j<u->window_size;++j){
298 // dst[j]=W[j]*src[j];
300 // //Processing is done here!
302 // fftwf_execute_dft_r2c(u->forward_plan,dst,output_window);
303 // //perform filtering
304 // for(size_t j=0;j<u->fft_size/2+1;++j){
305 // u->output_window[j][0]*=u->H[j];
306 // u->output_window[j][1]*=u->H[j];
309 // fftwf_execute_dft_c2r(u->inverse_plan,output_window,dst);
310 // ////debug: tests overlaping add
311 // ////and negates ALL PREVIOUS processing
312 // ////yields a perfect reconstruction if COLA is held
313 // //for(size_t j=0;j<u->window_size;++j){
314 // // u->work_buffer[j]=u->W[j]*u->input[c][j];
317 // //overlap add and preserve overlap component from this window (linear phase)
318 // for(size_t j=0;j<u->overlap_size;++j){
319 // u->work_buffer[j]+=overlap[j];
320 // overlap[j]=dst[u->R+j];
322 // ////debug: tests if basic buffering works
323 // ////shouldn't modify the signal AT ALL (beyond roundoff)
324 // //for(size_t j=0;j<u->window_size;++j){
325 // // u->work_buffer[j]=u->input[c][j];
328 // //preseve the needed input for the next window's overlap
329 // memmove(src,src+u->R,
330 // (u->samples_gathered+u->overlap_size-u->R)*sizeof(float)
334 //regardless of sse enabled, the loops in here assume
335 //16 byte aligned addresses and memory allocations divisible by v_size
337 float * __restrict__ dst
,//used as a temp array too, needs to be fft_length!
338 float * __restrict__ src
,/*input data w/ overlap at start,
339 *automatically cycled in routine
341 float * __restrict__ overlap
,//The size of the overlap
342 const float * __restrict__ H
,//The freq. magnitude scalers filter
343 const float * __restrict__ W
,//The windowing function
344 fftwf_complex
* __restrict__ output_window
,//The transformed window'd src
345 struct userdata
*u
){//Collection of constants
347 const size_t window_size
=mround(u
->window_size
,v_size
);
348 const size_t fft_h
=mround(u
->fft_size
/2+1,v_size
/2);
349 const size_t R
=mround(u
->R
,v_size
);
350 const size_t overlap_size
=mround(u
->overlap_size
,v_size
);
352 //assert(u->samples_gathered>=u->R);
353 //zero out the bit beyond the real overlap so we don't add garbage
354 for(size_t j
=overlap_size
;j
>u
->overlap_size
;--j
){
357 //use a linear-phase sliding STFT and overlap-add method
359 memset(dst
+u
->window_size
,0,(u
->fft_size
-u
->window_size
)*sizeof(float));
361 for(size_t j
=0;j
<window_size
;j
+=v_size
){
362 //dst[j]=W[j]*src[j];
363 float_vector_t
*d
=(float_vector_t
*)(dst
+j
);
364 float_vector_t
*w
=(float_vector_t
*)(W
+j
);
365 float_vector_t
*s
=(float_vector_t
*)(src
+j
);
367 d
->m
=_mm_mul_ps(w
->m
,s
->m
);
372 //Processing is done here!
374 fftwf_execute_dft_r2c(u
->forward_plan
,dst
,output_window
);
377 //perform filtering - purely magnitude based
378 for(size_t j
=0;j
<fft_h
;j
+=v_size
/2){
379 //output_window[j][0]*=H[j];
380 //output_window[j][1]*=H[j];
381 float_vector_t
*d
=(float_vector_t
*)(output_window
+j
);
384 h
.f
[2]=h
.f
[3]=H
[j
+1];
386 d
->m
=_mm_mul_ps(d
->m
,h
.m
);
394 fftwf_execute_dft_c2r(u
->inverse_plan
,output_window
,dst
);
396 ////debug: tests overlaping add
397 ////and negates ALL PREVIOUS processing
398 ////yields a perfect reconstruction if COLA is held
399 //for(size_t j=0;j<u->window_size;++j){
400 // dst[j]=W[j]*src[j];
403 //overlap add and preserve overlap component from this window (linear phase)
404 for(size_t j
=0;j
<overlap_size
;j
+=v_size
){
405 //dst[j]+=overlap[j];
406 //overlap[j]+=dst[j+R];
407 float_vector_t
*d
=(float_vector_t
*)(dst
+j
);
408 float_vector_t
*o
=(float_vector_t
*)(overlap
+j
);
410 d
->m
=_mm_add_ps(d
->m
,o
->m
);
411 o
->m
=((float_vector_t
*)(dst
+u
->R
+j
))->m
;
414 o
->v
=((float_vector_t
*)(dst
+u
->R
+j
))->v
;
417 //memcpy(overlap,dst+u->R,u->overlap_size*sizeof(float));
419 //////debug: tests if basic buffering works
420 //////shouldn't modify the signal AT ALL (beyond roundoff)
421 //for(size_t j=0;j<u->window_size;++j){
425 //preseve the needed input for the next window's overlap
426 memmove(src
,src
+u
->R
,
427 (u
->overlap_size
+u
->samples_gathered
-u
->R
)*sizeof(float)
433 void input_buffer(struct userdata
*u
,pa_memchunk
*in
){
434 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
435 size_t samples
=in
->length
/fs
;
436 pa_assert_se(samples
<=u
->target_samples
-u
->samples_gathered
);
437 float *src
= (float*) ((uint8_t*) pa_memblock_acquire(in
->memblock
) + in
->index
);
438 for (size_t c
=0;c
<u
->channels
;c
++) {
439 //buffer with an offset after the overlap from previous
442 u
->input
[c
]+u
->overlap_size
+u
->samples_gathered
+samples
<=u
->input
[c
]+u
->target_samples
+u
->overlap_size
444 pa_sample_clamp(PA_SAMPLE_FLOAT32NE
,u
->input
[c
]+u
->overlap_size
+u
->samples_gathered
,sizeof(float),src
+c
,fs
,samples
);
446 u
->samples_gathered
+=samples
;
447 pa_memblock_release(in
->memblock
);
450 /* Called from I/O thread context */
451 static int sink_input_pop_cb(pa_sink_input
*i
, size_t nbytes
, pa_memchunk
*chunk
) {
453 pa_sink_input_assert_ref(i
);
455 pa_assert_se(u
= i
->userdata
);
456 pa_assert_se(u
->sink
);
457 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
458 size_t samples_requested
=nbytes
/fs
;
459 size_t buffered_samples
=pa_memblockq_get_length(u
->rendered_q
)/fs
;
461 chunk
->memblock
=NULL
;
462 if (!u
->sink
|| !PA_SINK_IS_OPENED(u
->sink
->thread_info
.state
))
465 //pa_log("start output-buffered %ld, input-buffered %ld, requested %ld",buffered_samples,u->samples_gathered,samples_requested);
466 struct timespec start
,end
;
468 if(pa_memblockq_peek(u
->rendered_q
,&tchunk
)==0){
470 pa_memblockq_drop(u
->rendered_q
, chunk
->length
);
475 size_t input_remaining
=u
->target_samples
-u
->samples_gathered
;
476 pa_assert(input_remaining
>0);
479 buffer
=&u
->conv_buffer
;
480 buffer
->length
=input_remaining
*fs
;
482 pa_memblock_ref(buffer
->memblock
);
483 pa_sink_render_into(u
->sink
,buffer
);
485 //if(u->sink->thread_info.rewind_requested)
486 // sink_request_rewind(u->sink);
490 //pa_sink_render(u->sink,u->R*fs,buffer);
491 //buffer->length=PA_MIN(input_remaining*fs,buffer->length);
494 //pa_memblockq_push(u->rendered_q,buffer);
495 //pa_memblock_unref(buffer->memblock);
498 //pa_log("asked for %ld input samples, got %ld samples",input_remaining,buffer->length/fs);
501 input_buffer(u
,buffer
);
503 //pa_log("Took %0.5f seconds to setup",tdiff(end,start)*1e-9);
505 pa_memblock_unref(buffer
->memblock
);
507 pa_assert_se(u
->fft_size
>=u
->window_size
);
508 pa_assert_se(u
->R
<u
->window_size
);
509 //process every complete block on hand
514 //pa_log("Took %0.5f seconds to process",tdiff(end,start)*1e-9);
516 buffered_samples
=pa_memblockq_get_length(u
->rendered_q
)/fs
;
517 }while(buffered_samples
<u
->R
);
519 //deque from rendered_q and output
520 pa_assert_se(pa_memblockq_peek(u
->rendered_q
,&tchunk
)==0);
522 pa_memblockq_drop(u
->rendered_q
, chunk
->length
);
523 pa_assert_se(chunk
->memblock
);
524 //pa_log("gave %ld",chunk->length/fs);
529 /* Called from I/O thread context */
530 static void sink_input_process_rewind_cb(pa_sink_input
*i
, size_t nbytes
) {
534 pa_log_debug("Rewind callback!");
535 pa_sink_input_assert_ref(i
);
536 pa_assert_se(u
= i
->userdata
);
538 if (!u
->sink
|| !PA_SINK_IS_OPENED(u
->sink
->thread_info
.state
))
541 if (u
->sink
->thread_info
.rewind_nbytes
> 0) {
544 max_rewrite
= nbytes
+ pa_memblockq_get_length(u
->rendered_q
);
545 amount
= PA_MIN(u
->sink
->thread_info
.rewind_nbytes
, max_rewrite
);
546 u
->sink
->thread_info
.rewind_nbytes
= 0;
549 //pa_sample_spec *ss=&u->sink->sample_spec;
550 pa_memblockq_seek(u
->rendered_q
, - (int64_t) amount
, PA_SEEK_RELATIVE
, TRUE
);
551 pa_log_debug("Resetting equalizer");
552 u
->samples_gathered
=0;
556 pa_sink_process_rewind(u
->sink
, amount
);
557 pa_memblockq_rewind(u
->rendered_q
, nbytes
);
560 /* Called from I/O thread context */
561 static void sink_input_update_max_rewind_cb(pa_sink_input
*i
, size_t nbytes
) {
564 pa_sink_input_assert_ref(i
);
565 pa_assert_se(u
= i
->userdata
);
567 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
570 pa_memblockq_set_maxrewind(u
->rendered_q
, nbytes
);
571 pa_sink_set_max_rewind_within_thread(u
->sink
, nbytes
);
574 /* Called from I/O thread context */
575 static void sink_input_update_max_request_cb(pa_sink_input
*i
, size_t nbytes
) {
578 pa_sink_input_assert_ref(i
);
579 pa_assert_se(u
= i
->userdata
);
581 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
584 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
585 pa_sink_set_max_request_within_thread(u
->sink
, nbytes
);
586 //pa_sink_set_max_request_within_thread(u->sink, u->R*fs);
589 /* Called from I/O thread context */
590 static void sink_input_update_sink_latency_range_cb(pa_sink_input
*i
) {
593 pa_sink_input_assert_ref(i
);
594 pa_assert_se(u
= i
->userdata
);
596 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
599 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
600 pa_sink_set_latency_range_within_thread(u
->sink
, u
->master
->thread_info
.min_latency
, u
->latency
*fs
);
601 //pa_sink_set_latency_range_within_thread(u->sink,u->latency*fs ,u->latency*fs );
602 //pa_sink_set_latency_range_within_thread(u->sink, i->sink->thread_info.min_latency, i->sink->thread_info.max_latency);
605 /* Called from I/O thread context */
606 static void sink_input_detach_cb(pa_sink_input
*i
) {
609 pa_sink_input_assert_ref(i
);
610 pa_assert_se(u
= i
->userdata
);
612 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
615 pa_sink_detach_within_thread(u
->sink
);
616 pa_sink_set_asyncmsgq(u
->sink
, NULL
);
617 pa_sink_set_rtpoll(u
->sink
, NULL
);
620 /* Called from I/O thread context */
621 static void sink_input_attach_cb(pa_sink_input
*i
) {
624 pa_sink_input_assert_ref(i
);
625 pa_assert_se(u
= i
->userdata
);
627 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
630 pa_sink_set_asyncmsgq(u
->sink
, i
->sink
->asyncmsgq
);
631 pa_sink_set_rtpoll(u
->sink
, i
->sink
->rtpoll
);
632 pa_sink_attach_within_thread(u
->sink
);
634 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
635 //pa_sink_set_latency_range_within_thread(u->sink, u->latency*fs, u->latency*fs);
636 //pa_sink_set_latency_range_within_thread(u->sink,u->latency*fs, u->master->thread_info.max_latency);
637 //TODO: setting this guy minimizes drop outs but doesn't get rid
638 //of them completely, figure out why
639 pa_sink_set_latency_range_within_thread(u
->sink
, u
->master
->thread_info
.min_latency
, u
->latency
*fs
);
640 //TODO: this guy causes dropouts constantly+rewinds, it's unusable
641 //pa_sink_set_latency_range_within_thread(u->sink, u->master->thread_info.min_latency, u->master->thread_info.max_latency);
644 /* Called from main context */
645 static void sink_input_kill_cb(pa_sink_input
*i
) {
648 pa_sink_input_assert_ref(i
);
649 pa_assert_se(u
= i
->userdata
);
651 pa_sink_unlink(u
->sink
);
652 pa_sink_input_unlink(u
->sink_input
);
654 pa_sink_unref(u
->sink
);
656 pa_sink_input_unref(u
->sink_input
);
657 u
->sink_input
= NULL
;
659 pa_module_unload_request(u
->module
, TRUE
);
662 /* Called from IO thread context */
663 static void sink_input_state_change_cb(pa_sink_input
*i
, pa_sink_input_state_t state
) {
666 pa_sink_input_assert_ref(i
);
667 pa_assert_se(u
= i
->userdata
);
669 /* If we are added for the first time, ask for a rewinding so that
670 * we are heard right-away. */
671 if (PA_SINK_INPUT_IS_LINKED(state
) &&
672 i
->thread_info
.state
== PA_SINK_INPUT_INIT
) {
673 pa_log_debug("Requesting rewind due to state change.");
674 pa_sink_input_request_rewind(i
, 0, FALSE
, TRUE
, TRUE
);
678 /* Called from main context */
679 static pa_bool_t
sink_input_may_move_to_cb(pa_sink_input
*i
, pa_sink
*dest
) {
682 pa_sink_input_assert_ref(i
);
683 pa_assert_se(u
= i
->userdata
);
685 return u
->sink
!= dest
;
689 //ensure's memory allocated is a multiple of v_size
691 static void * alloc(size_t x
,size_t s
){
692 size_t f
=mround(x
*s
,sizeof(float)*v_size
);
693 //printf("requested %ld floats=%ld bytes, rem=%ld\n",x,x*sizeof(float),x*sizeof(float)%16);
694 //printf("giving %ld floats=%ld bytes, rem=%ld\n",f,f*sizeof(float),f*sizeof(float)%16);
695 return fftwf_malloc(f
*s
);
698 int pa__init(pa_module
*m
) {
705 pa_sink_input_new_data sink_input_data
;
706 pa_sink_new_data sink_data
;
707 pa_bool_t
*use_default
= NULL
;
712 if (!(ma
= pa_modargs_new(m
->argument
, valid_modargs
))) {
713 pa_log("Failed to parse module arguments.");
717 if (!(master
= pa_namereg_get(m
->core
, pa_modargs_get_value(ma
, "master", NULL
), PA_NAMEREG_SINK
))) {
718 pa_log("Master sink not found");
722 ss
= master
->sample_spec
;
723 ss
.format
= PA_SAMPLE_FLOAT32
;
724 map
= master
->channel_map
;
725 if (pa_modargs_get_sample_spec_and_channel_map(ma
, &ss
, &map
, PA_CHANNEL_MAP_DEFAULT
) < 0) {
726 pa_log("Invalid sample format specification or channel map");
729 fs
=pa_frame_size(&ss
);
731 u
= pa_xnew0(struct userdata
, 1);
737 u
->sink_input
= NULL
;
739 u
->channels
=ss
.channels
;
740 u
->fft_size
=pow(2,ceil(log(ss
.rate
)/log(2)));
741 pa_log("fft size: %ld",u
->fft_size
);
742 u
->window_size
=15999;
743 u
->R
=(u
->window_size
+1)/2;
744 u
->overlap_size
=u
->window_size
-u
->R
;
745 u
->target_samples
=1*u
->R
;
746 u
->samples_gathered
=0;
747 u
->max_output
=pa_frame_align(pa_mempool_block_size_max(m
->core
->mempool
), &ss
)/pa_frame_size(&ss
);
748 u
->rendered_q
= pa_memblockq_new(0, MEMBLOCKQ_MAXLENGTH
,u
->target_samples
*fs
, fs
, fs
, 0, 0, NULL
);
749 u
->conv_buffer
.memblock
=pa_memblock_new(u
->core
->mempool
,u
->target_samples
*fs
);
752 u
->H
=alloc((u
->fft_size
/2+1),sizeof(fftwf_complex
));
753 u
->W
=alloc(u
->window_size
,sizeof(float));
754 u
->work_buffer
=alloc(u
->fft_size
,sizeof(float));
755 memset(u
->work_buffer
,0,u
->fft_size
*sizeof(float));
756 u
->input
=(float **)malloc(sizeof(float *)*u
->channels
);
757 u
->overlap_accum
=(float **)malloc(sizeof(float *)*u
->channels
);
758 u
->output_buffer
=(float **)malloc(sizeof(float *)*u
->channels
);
759 for(size_t c
=0;c
<u
->channels
;++c
){
760 u
->input
[c
]=alloc(u
->target_samples
+u
->overlap_size
,sizeof(float));
761 pa_assert_se(u
->input
[c
]);
762 memset(u
->input
[c
],0,(u
->target_samples
+u
->overlap_size
)*sizeof(float));
763 pa_assert_se(u
->input
[c
]);
764 u
->overlap_accum
[c
]=alloc(u
->overlap_size
,sizeof(float));
765 pa_assert_se(u
->overlap_accum
[c
]);
766 memset(u
->overlap_accum
[c
],0,u
->overlap_size
*sizeof(float));
767 u
->output_buffer
[c
]=alloc(u
->window_size
,sizeof(float));
768 pa_assert_se(u
->output_buffer
[c
]);
770 u
->output_window
=alloc((u
->fft_size
/2+1),sizeof(fftwf_complex
));
771 u
->forward_plan
=fftwf_plan_dft_r2c_1d(u
->fft_size
, u
->work_buffer
, u
->output_window
, FFTW_MEASURE
);
772 u
->inverse_plan
=fftwf_plan_dft_c2r_1d(u
->fft_size
, u
->output_window
, u
->work_buffer
, FFTW_MEASURE
);
774 hanning_window(u
->W
,u
->window_size
);
776 const int freqs
[]={0,25,50,100,200,300,400,800,1500,
777 2000,3000,4000,5000,6000,7000,8000,9000,10000,11000,12000,
778 13000,14000,15000,16000,17000,18000,19000,20000,21000,22000,23000,24000,INT_MAX
};
779 const float coefficients
[]={1,1,1,1,1,1,1,1,1,1,
781 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
782 const size_t ncoefficients
=sizeof(coefficients
)/sizeof(float);
783 pa_assert_se(sizeof(freqs
)/sizeof(int)==sizeof(coefficients
)/sizeof(float));
784 float *freq_translated
=(float *) malloc(sizeof(float)*(ncoefficients
));
785 freq_translated
[0]=1;
786 //Translate the frequencies in their natural sampling rate to the new sampling rate frequencies
787 for(size_t i
=1;i
<ncoefficients
-1;++i
){
788 freq_translated
[i
]=((float)freqs
[i
]*u
->fft_size
)/ss
.rate
;
789 //pa_log("i: %ld: %d , %g",i,freqs[i],freq_translated[i]);
790 pa_assert_se(freq_translated
[i
]>=freq_translated
[i
-1]);
792 freq_translated
[ncoefficients
-1]=FLT_MAX
;
793 //Interpolate the specified frequency band values
795 for(size_t i
=1,j
=0;i
<(u
->fft_size
/2+1);++i
){
796 pa_assert_se(j
<ncoefficients
);
797 //max frequency range passed, consider the rest as one band
798 if(freq_translated
[j
+1]>=FLT_MAX
){
799 for(;i
<(u
->fft_size
/2+1);++i
){
800 u
->H
[i
]=coefficients
[j
];
804 //pa_log("i: %d, j: %d, freq: %f",i,j,freq_translated[j]);
805 //pa_log("interp: %0.4f %0.4f",freq_translated[j],freq_translated[j+1]);
806 pa_assert_se(freq_translated
[j
]<freq_translated
[j
+1]);
807 pa_assert_se(i
>=freq_translated
[j
]);
808 pa_assert_se(i
<=freq_translated
[j
+1]);
809 //bilinear-inerpolation of coefficients specified
810 float c0
=(i
-freq_translated
[j
])/(freq_translated
[j
+1]-freq_translated
[j
]);
811 pa_assert_se(c0
>=0&&c0
<=1.0);
812 u
->H
[i
]=((1.0f
-c0
)*coefficients
[j
]+c0
*coefficients
[j
+1]);
813 pa_assert_se(u
->H
[i
]>0);
814 while(i
>=floor(freq_translated
[j
+1])){
818 //divide out the fft gain
819 for(size_t i
=0;i
<(u
->fft_size
/2+1);++i
){
820 u
->H
[i
]/=u
->fft_size
;
822 free(freq_translated
);
826 pa_sink_new_data_init(&sink_data
);
827 sink_data
.driver
= __FILE__
;
828 sink_data
.module
= m
;
829 if (!(sink_data
.name
= pa_xstrdup(pa_modargs_get_value(ma
, "sink_name", NULL
))))
830 sink_data
.name
= pa_sprintf_malloc("%s.equalizer", master
->name
);
831 sink_data
.namereg_fail
= FALSE
;
832 pa_sink_new_data_set_sample_spec(&sink_data
, &ss
);
833 pa_sink_new_data_set_channel_map(&sink_data
, &map
);
834 z
= pa_proplist_gets(master
->proplist
, PA_PROP_DEVICE_DESCRIPTION
);
835 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_DESCRIPTION
, "FFT based equalizer");
836 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_MASTER_DEVICE
, master
->name
);
837 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_CLASS
, "filter");
839 if (pa_modargs_get_proplist(ma
, "sink_properties", sink_data
.proplist
, PA_UPDATE_REPLACE
) < 0) {
840 pa_log("Invalid properties");
841 pa_sink_new_data_done(&sink_data
);
845 u
->sink
= pa_sink_new(m
->core
, &sink_data
, PA_SINK_LATENCY
|PA_SINK_DYNAMIC_LATENCY
);
846 pa_sink_new_data_done(&sink_data
);
849 pa_log("Failed to create sink.");
853 u
->sink
->parent
.process_msg
= sink_process_msg
;
854 u
->sink
->set_state
= sink_set_state
;
855 u
->sink
->update_requested_latency
= sink_update_requested_latency
;
856 u
->sink
->request_rewind
= sink_request_rewind
;
857 u
->sink
->userdata
= u
;
859 pa_sink_set_asyncmsgq(u
->sink
, master
->asyncmsgq
);
860 pa_sink_set_rtpoll(u
->sink
, master
->rtpoll
);
861 pa_sink_set_max_request(u
->sink
,u
->R
*fs
);
862 //pa_sink_set_fixed_latency(u->sink,pa_bytes_to_usec(u->R*fs,&ss));
864 /* Create sink input */
865 pa_sink_input_new_data_init(&sink_input_data
);
866 sink_input_data
.driver
= __FILE__
;
867 sink_input_data
.module
= m
;
868 sink_input_data
.sink
= u
->master
;
869 pa_proplist_sets(sink_input_data
.proplist
, PA_PROP_MEDIA_NAME
, "Equalized Stream");
870 pa_proplist_sets(sink_input_data
.proplist
, PA_PROP_MEDIA_ROLE
, "filter");
871 pa_sink_input_new_data_set_sample_spec(&sink_input_data
, &ss
);
872 pa_sink_input_new_data_set_channel_map(&sink_input_data
, &map
);
874 pa_sink_input_new(&u
->sink_input
, m
->core
, &sink_input_data
, PA_SINK_INPUT_DONT_MOVE
);
875 pa_sink_input_new_data_done(&sink_input_data
);
880 u
->sink_input
->pop
= sink_input_pop_cb
;
881 u
->sink_input
->process_rewind
= sink_input_process_rewind_cb
;
882 u
->sink_input
->update_max_rewind
= sink_input_update_max_rewind_cb
;
883 u
->sink_input
->update_max_request
= sink_input_update_max_request_cb
;
884 u
->sink_input
->update_sink_latency_range
= sink_input_update_sink_latency_range_cb
;
885 u
->sink_input
->kill
= sink_input_kill_cb
;
886 u
->sink_input
->attach
= sink_input_attach_cb
;
887 u
->sink_input
->detach
= sink_input_detach_cb
;
888 u
->sink_input
->state_change
= sink_input_state_change_cb
;
889 u
->sink_input
->may_move_to
= sink_input_may_move_to_cb
;
890 u
->sink_input
->userdata
= u
;
892 pa_sink_put(u
->sink
);
893 pa_sink_input_put(u
->sink_input
);
897 pa_xfree(use_default
);
905 pa_xfree(use_default
);
912 int pa__get_n_used(pa_module
*m
) {
916 pa_assert_se(u
= m
->userdata
);
918 return pa_sink_linked_by(u
->sink
);
921 void pa__done(pa_module
*m
) {
926 if (!(u
= m
->userdata
))
930 pa_sink_unlink(u
->sink
);
931 pa_sink_unref(u
->sink
);
935 pa_sink_input_unlink(u
->sink_input
);
936 pa_sink_input_unref(u
->sink_input
);
939 if(u
->conv_buffer
.memblock
)
940 pa_memblock_unref(u
->conv_buffer
.memblock
);
943 pa_memblockq_free(u
->rendered_q
);
945 fftwf_destroy_plan(u
->inverse_plan
);
946 fftwf_destroy_plan(u
->forward_plan
);
947 free(u
->output_window
);
948 for(size_t c
=0;c
<u
->channels
;++c
){
949 free(u
->output_buffer
[c
]);
950 free(u
->overlap_accum
[c
]);
953 free(u
->output_buffer
);
954 free(u
->overlap_accum
);
956 free(u
->work_buffer
);