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
39 #include <pulse/xmalloc.h>
40 #include <pulse/i18n.h>
42 #include <pulsecore/core-error.h>
43 #include <pulsecore/namereg.h>
44 #include <pulsecore/sink.h>
45 #include <pulsecore/module.h>
46 #include <pulsecore/core-util.h>
47 #include <pulsecore/modargs.h>
48 #include <pulsecore/log.h>
49 #include <pulsecore/thread.h>
50 #include <pulsecore/thread-mq.h>
51 #include <pulsecore/rtpoll.h>
52 #include <pulsecore/sample-util.h>
53 #include <pulsecore/ltdl-helper.h>
54 #include <pulsecore/protocol-dbus.h>
55 #include <pulsecore/dbus-util.h>
63 #include <xmmintrin.h>
64 #include <emmintrin.h>
69 #include "module-equalizer-sink-symdef.h"
71 PA_MODULE_AUTHOR("Jason Newton");
72 PA_MODULE_DESCRIPTION(_("General Purpose Equalizer"));
73 PA_MODULE_VERSION(PACKAGE_VERSION
);
74 PA_MODULE_LOAD_ONCE(FALSE
);
75 PA_MODULE_USAGE(_("sink=<sink to connect to> "));
77 #define MEMBLOCKQ_MAXLENGTH (16*1024*1024)
83 pa_sink
*sink
, *master
;
84 pa_sink_input
*sink_input
;
87 size_t fft_size
;//length (res) of fft
90 *effectively chooses R
92 size_t R
;/* the hop size between overlapping windows
93 * the latency of the filter, calculated from window_size
94 * based on constraints of COLA and window function
96 size_t latency
;//Really just R but made into it's own variable
97 //for twiddling with pulseaudio
98 size_t overlap_size
;//window_size-R
99 size_t samples_gathered
;
100 size_t max_output
;//max amount of samples outputable in a single
102 size_t target_samples
;
103 float *H
;//frequency response filter (magnitude based)
104 float *W
;//windowing function (time domain)
105 float *work_buffer
, **input
, **overlap_accum
;
106 fftwf_complex
*output_window
;
107 fftwf_plan forward_plan
, inverse_plan
;
110 float *Hs
[2];//thread updatable copies
112 pa_memchunk conv_buffer
;
113 pa_memblockq
*rendered_q
;
115 pa_dbus_protocol
*dbus_protocol
;
119 static const char* const valid_modargs
[] = {
130 static uint64_t time_diff(struct timespec
*timeA_p
, struct timespec
*timeB_p
);
131 static void hanning_window(float *W
, size_t window_size
);
132 static void array_out(const char *name
, float *a
, size_t length
);
133 static void process_samples(struct userdata
*u
);
134 static void input_buffer(struct userdata
*u
, pa_memchunk
*in
);
137 float * __restrict__ dst
,
138 float * __restrict__ src
,
139 float * __restrict__ overlap
,
140 const float * __restrict__ H
,
141 const float * __restrict__ W
,
142 fftwf_complex
* __restrict__ output_window
,
145 static void dbus_init(struct userdata
*u
);
146 static void dbus_done(struct userdata
*u
);
147 static void handle_get_all(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
);
148 static void get_n_coefs(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
);
149 static void get_filter(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
);
150 static void set_filter(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
);
153 #define gettime(x) clock_gettime(CLOCK_MONOTONIC, &x)
154 #define tdiff(x, y) time_diff(&x, &y)
155 #define mround(x, y) (x % y == 0 ? x : ( x / y + 1) * y)
157 uint64_t time_diff(struct timespec
*timeA_p
, struct timespec
*timeB_p
)
159 return ((timeA_p
->tv_sec
* 1000000000ULL) + timeA_p
->tv_nsec
) -
160 ((timeB_p
->tv_sec
* 1000000000ULL) + timeB_p
->tv_nsec
);
163 static void hanning_window(float *W
, size_t window_size
){
164 //h=.5*(1-cos(2*pi*j/(window_size+1)), COLA for R=(M+1)/2
165 for(size_t i
=0; i
< window_size
;++i
){
166 W
[i
] = (float).5*(1-cos(2*M_PI
*i
/(window_size
+1)));
170 static void fix_filter(float *H
, size_t fft_size
){
171 //divide out the fft gain
172 for(size_t i
= 0; i
< (fft_size
/ 2 + 1); ++i
){
177 void array_out(const char *name
, float *a
, size_t length
){
178 FILE *p
=fopen(name
, "w");
180 pa_log("opening %s failed!", name
);
183 for(size_t i
= 0; i
< length
; ++i
){
184 fprintf(p
, "%e,", a
[i
]);
194 /* Called from I/O thread context */
195 static int sink_process_msg(pa_msgobject
*o
, int code
, void *data
, int64_t offset
, pa_memchunk
*chunk
) {
196 struct userdata
*u
= PA_SINK(o
)->userdata
;
200 case PA_SINK_MESSAGE_GET_LATENCY
: {
202 pa_sample_spec
*ss
=&u
->sink
->sample_spec
;
203 //size_t fs=pa_frame_size(&(u->sink->sample_spec));
205 /* Get the latency of the master sink */
206 if (PA_MSGOBJECT(u
->master
)->process_msg(PA_MSGOBJECT(u
->master
), PA_SINK_MESSAGE_GET_LATENCY
, &usec
, 0, NULL
) < 0)
209 //usec+=pa_bytes_to_usec(u->latency * fs, ss);
210 //usec+=pa_bytes_to_usec(u->samples_gathered * fs, ss);
211 usec
+= pa_bytes_to_usec(pa_memblockq_get_length(u
->rendered_q
), ss
);
212 /* Add the latency internal to our sink input on top */
213 usec
+= pa_bytes_to_usec(pa_memblockq_get_length(u
->sink_input
->thread_info
.render_memblockq
), &u
->master
->sample_spec
);
214 *((pa_usec_t
*) data
) = usec
;
219 return pa_sink_process_msg(o
, code
, data
, offset
, chunk
);
223 /* Called from main context */
224 static int sink_set_state(pa_sink
*s
, pa_sink_state_t state
) {
227 pa_sink_assert_ref(s
);
228 pa_assert_se(u
= s
->userdata
);
230 if (PA_SINK_IS_LINKED(state
) &&
232 PA_SINK_INPUT_IS_LINKED(pa_sink_input_get_state(u
->sink_input
)))
234 pa_sink_input_cork(u
->sink_input
, state
== PA_SINK_SUSPENDED
);
239 /* Called from I/O thread context */
240 static void sink_request_rewind(pa_sink
*s
) {
243 pa_sink_assert_ref(s
);
244 pa_assert_se(u
= s
->userdata
);
246 /* Just hand this one over to the master sink */
247 pa_sink_input_request_rewind(u
->sink_input
, s
->thread_info
.rewind_nbytes
+ pa_memblockq_get_length(u
->rendered_q
), TRUE
, FALSE
, FALSE
);
250 /* Called from I/O thread context */
251 static void sink_update_requested_latency(pa_sink
*s
) {
254 pa_sink_assert_ref(s
);
255 pa_assert_se(u
= s
->userdata
);
257 /* Just hand this one over to the master sink */
258 pa_sink_input_set_requested_latency_within_thread(
260 pa_sink_get_requested_latency_within_thread(s
));
263 static void process_samples(struct userdata
*u
){
265 size_t fs
=pa_frame_size(&(u
->sink
->sample_spec
));
266 while(u
->samples_gathered
>= u
->R
){
268 //pa_log("iter gathered: %ld", u->samples_gathered);
269 //pa_memblockq_drop(u->rendered_q, tchunk.length);
271 tchunk
.length
=u
->R
*fs
;
272 tchunk
.memblock
=pa_memblock_new(u
->core
->mempool
, tchunk
.length
);
273 dst
=((float*)pa_memblock_acquire(tchunk
.memblock
));
274 for(size_t c
=0;c
< u
->channels
; c
++) {
284 pa_sample_clamp(PA_SAMPLE_FLOAT32NE
, dst
+ c
, fs
, u
->work_buffer
, sizeof(float), u
->R
);
286 pa_memblock_release(tchunk
.memblock
);
287 pa_memblockq_push(u
->rendered_q
, &tchunk
);
288 pa_memblock_unref(tchunk
.memblock
);
289 u
->samples_gathered
-=u
->R
;
293 typedef float v4sf
__attribute__ ((__aligned__(v_size
*sizeof(float))));
294 typedef union float_vector
{
302 //reference implementation
304 float * __restrict__ dst
,//used as a temp array too, needs to be fft_length!
305 float * __restrict__ src
,/*input data w/ overlap at start,
306 *automatically cycled in routine
308 float * __restrict__ overlap
,//The size of the overlap
309 const float * __restrict__ H
,//The freq. magnitude scalers filter
310 const float * __restrict__ W
,//The windowing function
311 fftwf_complex
* __restrict__ output_window
,//The transformed window'd src
313 //use a linear-phase sliding STFT and overlap-add method (for each channel)
315 memset(dst
+ u
->window_size
, 0, (u
->fft_size
- u
->window_size
) * sizeof(float));
317 for(size_t j
= 0;j
< u
->window_size
; ++j
){
318 dst
[j
] = W
[j
] * src
[j
];
320 //Processing is done here!
322 fftwf_execute_dft_r2c(u
->forward_plan
, dst
, output_window
);
324 for(size_t j
= 0;j
< u
->fft_size
/ 2 + 1; ++j
){
325 u
->output_window
[j
][0] *= u
->H
[j
];
326 u
->output_window
[j
][1] *= u
->H
[j
];
329 fftwf_execute_dft_c2r(u
->inverse_plan
, output_window
, dst
);
330 ////debug: tests overlaping add
331 ////and negates ALL PREVIOUS processing
332 ////yields a perfect reconstruction if COLA is held
333 //for(size_t j = 0; j < u->window_size; ++j){
334 // u->work_buffer[j] = u->W[j] * u->input[c][j];
337 //overlap add and preserve overlap component from this window (linear phase)
338 for(size_t j
= 0;j
< u
->overlap_size
; ++j
){
339 u
->work_buffer
[j
] += overlap
[j
];
340 overlap
[j
] = dst
[u
->R
+j
];
342 ////debug: tests if basic buffering works
343 ////shouldn't modify the signal AT ALL (beyond roundoff)
344 //for(size_t j = 0; j < u->window_size;++j){
345 // u->work_buffer[j] = u->input[c][j];
348 //preseve the needed input for the next window's overlap
349 memmove(src
, src
+u
->R
,
350 ((u
->overlap_size
+ u
->samples_gathered
) - u
->R
)*sizeof(float)
354 ////regardless of sse enabled, the loops in here assume
355 ////16 byte aligned addresses and memory allocations divisible by v_size
357 // float * __restrict__ dst,//used as a temp array too, needs to be fft_length!
358 // float * __restrict__ src,/*input data w/ overlap at start,
359 // *automatically cycled in routine
361 // float * __restrict__ overlap,//The size of the overlap
362 // const float * __restrict__ H,//The freq. magnitude scalers filter
363 // const float * __restrict__ W,//The windowing function
364 // fftwf_complex * __restrict__ output_window,//The transformed window'd src
365 // struct userdata *u){//Collection of constants
367 // const size_t window_size = mround(u->window_size,v_size);
368 // const size_t fft_h = mround(u->fft_size / 2 + 1, v_size / 2);
369 // //const size_t R = mround(u->R, v_size);
370 // const size_t overlap_size = mround(u->overlap_size, v_size);
372 // //assert(u->samples_gathered >= u->R);
373 // //zero out the bit beyond the real overlap so we don't add garbage
374 // for(size_t j = overlap_size; j > u->overlap_size; --j){
377 // //use a linear-phase sliding STFT and overlap-add method
378 // //zero padd the data
379 // memset(dst + u->window_size, 0, (u->fft_size - u->window_size)*sizeof(float));
381 // for(size_t j = 0; j < window_size; j += v_size){
382 // //dst[j] = W[j]*src[j];
383 // float_vector_t *d = (float_vector_t*) (dst+j);
384 // float_vector_t *w = (float_vector_t*) (W+j);
385 // float_vector_t *s = (float_vector_t*) (src+j);
387 // d->m = _mm_mul_ps(w->m, s->m);
389 // d->v = w->v * s->v;
392 // //Processing is done here!
394 // fftwf_execute_dft_r2c(u->forward_plan, dst, output_window);
397 // //perform filtering - purely magnitude based
398 // for(size_t j = 0;j < fft_h; j+=v_size/2){
399 // //output_window[j][0]*=H[j];
400 // //output_window[j][1]*=H[j];
401 // float_vector_t *d = (float_vector_t*)(output_window+j);
403 // h.f[0] = h.f[1] = H[j];
404 // h.f[2] = h.f[3] = H[j+1];
406 // d->m = _mm_mul_ps(d->m, h.m);
414 // fftwf_execute_dft_c2r(u->inverse_plan, output_window, dst);
416 // ////debug: tests overlaping add
417 // ////and negates ALL PREVIOUS processing
418 // ////yields a perfect reconstruction if COLA is held
419 // //for(size_t j = 0; j < u->window_size; ++j){
420 // // dst[j] = W[j]*src[j];
423 // //overlap add and preserve overlap component from this window (linear phase)
424 // for(size_t j = 0; j < overlap_size; j+=v_size){
425 // //dst[j]+=overlap[j];
426 // //overlap[j]+=dst[j+R];
427 // float_vector_t *d = (float_vector_t*)(dst+j);
428 // float_vector_t *o = (float_vector_t*)(overlap+j);
430 // d->m = _mm_add_ps(d->m, o->m);
431 // o->m = ((float_vector_t*)(dst+u->R+j))->m;
434 // o->v = ((float_vector_t*)(dst+u->R+j))->v;
437 // //memcpy(overlap, dst+u->R, u->overlap_size*sizeof(float));
439 // //////debug: tests if basic buffering works
440 // //////shouldn't modify the signal AT ALL (beyond roundoff)
441 // //for(size_t j = 0; j < u->window_size; ++j){
442 // // dst[j] = src[j];
445 // //preseve the needed input for the next window's overlap
446 // memmove(src, src+u->R,
447 // ((u->overlap_size+u->samples_gathered)+-u->R)*sizeof(float)
453 void input_buffer(struct userdata
*u
, pa_memchunk
*in
){
454 size_t fs
= pa_frame_size(&(u
->sink
->sample_spec
));
455 size_t samples
= in
->length
/fs
;
456 pa_assert_se(samples
<= u
->target_samples
-u
->samples_gathered
);
457 float *src
= (float*) ((uint8_t*) pa_memblock_acquire(in
->memblock
) + in
->index
);
458 for(size_t c
= 0; c
< u
->channels
; c
++) {
459 //buffer with an offset after the overlap from previous
462 u
->input
[c
]+u
->overlap_size
+u
->samples_gathered
+samples
<= u
->input
[c
]+u
->overlap_size
+u
->target_samples
464 pa_sample_clamp(PA_SAMPLE_FLOAT32NE
, u
->input
[c
]+u
->overlap_size
+u
->samples_gathered
, sizeof(float), src
+ c
, fs
, samples
);
466 u
->samples_gathered
+=samples
;
467 pa_memblock_release(in
->memblock
);
470 /* Called from I/O thread context */
471 static int sink_input_pop_cb(pa_sink_input
*i
, size_t nbytes
, pa_memchunk
*chunk
) {
473 pa_sink_input_assert_ref(i
);
475 pa_assert_se(u
= i
->userdata
);
476 pa_assert_se(u
->sink
);
477 size_t fs
= pa_frame_size(&(u
->sink
->sample_spec
));
478 //size_t samples_requested = nbytes/fs;
479 size_t buffered_samples
= pa_memblockq_get_length(u
->rendered_q
)/fs
;
481 chunk
->memblock
= NULL
;
482 if (!u
->sink
|| !PA_SINK_IS_OPENED(u
->sink
->thread_info
.state
))
485 //pa_log("start output-buffered %ld, input-buffered %ld, requested %ld",buffered_samples,u->samples_gathered,samples_requested);
486 struct timespec start
, end
;
488 if(pa_memblockq_peek(u
->rendered_q
, &tchunk
)==0){
490 pa_memblockq_drop(u
->rendered_q
, chunk
->length
);
497 unsigned H_i
= pa_aupdate_read_begin(u
->a_H
);
502 size_t input_remaining
= u
->target_samples
-u
->samples_gathered
;
503 pa_assert(input_remaining
>0);
506 buffer
= &u
->conv_buffer
;
507 buffer
->length
= input_remaining
*fs
;
509 pa_memblock_ref(buffer
->memblock
);
510 pa_sink_render_into(u
->sink
, buffer
);
512 //if(u->sink->thread_info.rewind_requested)
513 // sink_request_rewind(u->sink);
517 //pa_sink_render(u->sink, u->R*fs, buffer);
518 //buffer->length = PA_MIN(input_remaining*fs, buffer->length);
521 //pa_memblockq_push(u->rendered_q, buffer);
522 //pa_memblock_unref(buffer->memblock);
525 //pa_log("asked for %ld input samples, got %ld samples",input_remaining,buffer->length/fs);
528 input_buffer(u
, buffer
);
530 //pa_log("Took %0.5f seconds to setup", tdiff(end, start)*1e-9);
532 pa_memblock_unref(buffer
->memblock
);
534 pa_assert_se(u
->fft_size
>= u
->window_size
);
535 pa_assert_se(u
->R
< u
->window_size
);
536 //process every complete block on hand
541 //pa_log("Took %0.5f seconds to process", tdiff(end, start)*1e-9);
543 buffered_samples
= pa_memblockq_get_length(u
->rendered_q
)/fs
;
544 }while(buffered_samples
< u
->R
);
546 //deque from rendered_q and output
547 pa_assert_se(pa_memblockq_peek(u
->rendered_q
, &tchunk
)==0);
549 pa_memblockq_drop(u
->rendered_q
, chunk
->length
);
550 pa_assert_se(chunk
->memblock
);
551 //pa_log("gave %ld", chunk->length/fs);
556 /* Called from I/O thread context */
557 static void sink_input_process_rewind_cb(pa_sink_input
*i
, size_t nbytes
) {
561 pa_log_debug("Rewind callback!");
562 pa_sink_input_assert_ref(i
);
563 pa_assert_se(u
= i
->userdata
);
565 if (!u
->sink
|| !PA_SINK_IS_OPENED(u
->sink
->thread_info
.state
))
568 if (u
->sink
->thread_info
.rewind_nbytes
> 0) {
571 max_rewrite
= nbytes
+ pa_memblockq_get_length(u
->rendered_q
);
572 amount
= PA_MIN(u
->sink
->thread_info
.rewind_nbytes
, max_rewrite
);
573 u
->sink
->thread_info
.rewind_nbytes
= 0;
576 //pa_sample_spec *ss = &u->sink->sample_spec;
577 pa_memblockq_seek(u
->rendered_q
, - (int64_t) amount
, PA_SEEK_RELATIVE
, TRUE
);
578 pa_log_debug("Resetting equalizer");
579 u
->samples_gathered
= 0;
583 pa_sink_process_rewind(u
->sink
, amount
);
584 pa_memblockq_rewind(u
->rendered_q
, nbytes
);
587 /* Called from I/O thread context */
588 static void sink_input_update_max_rewind_cb(pa_sink_input
*i
, size_t nbytes
) {
591 pa_sink_input_assert_ref(i
);
592 pa_assert_se(u
= i
->userdata
);
594 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
597 pa_memblockq_set_maxrewind(u
->rendered_q
, nbytes
);
598 pa_sink_set_max_rewind_within_thread(u
->sink
, nbytes
);
601 /* Called from I/O thread context */
602 static void sink_input_update_max_request_cb(pa_sink_input
*i
, size_t nbytes
) {
605 pa_sink_input_assert_ref(i
);
606 pa_assert_se(u
= i
->userdata
);
608 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
611 size_t fs
= pa_frame_size(&(u
->sink
->sample_spec
));
612 //pa_sink_set_max_request_within_thread(u->sink, nbytes);
613 pa_sink_set_max_request_within_thread(u
->sink
, u
->R
*fs
);
616 /* Called from I/O thread context */
617 static void sink_input_update_sink_latency_range_cb(pa_sink_input
*i
) {
620 pa_sink_input_assert_ref(i
);
621 pa_assert_se(u
= i
->userdata
);
623 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
626 size_t fs
= pa_frame_size(&(u
->sink
->sample_spec
));
627 //pa_sink_set_latency_range_within_thread(u->sink, u->master->thread_info.min_latency, u->latency*fs);
628 pa_sink_set_latency_range_within_thread(u
->sink
, u
->latency
*fs
, u
->latency
*fs
);
629 //pa_sink_set_latency_range_within_thread(u->sink, i->sink->thread_info.min_latency, i->sink->thread_info.max_latency);
632 /* Called from I/O thread context */
633 static void sink_input_detach_cb(pa_sink_input
*i
) {
636 pa_sink_input_assert_ref(i
);
637 pa_assert_se(u
= i
->userdata
);
639 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
642 pa_sink_detach_within_thread(u
->sink
);
643 pa_sink_set_asyncmsgq(u
->sink
, NULL
);
644 pa_sink_set_rtpoll(u
->sink
, NULL
);
647 /* Called from I/O thread context */
648 static void sink_input_attach_cb(pa_sink_input
*i
) {
651 pa_sink_input_assert_ref(i
);
652 pa_assert_se(u
= i
->userdata
);
654 if (!u
->sink
|| !PA_SINK_IS_LINKED(u
->sink
->thread_info
.state
))
657 pa_sink_set_asyncmsgq(u
->sink
, i
->sink
->asyncmsgq
);
658 pa_sink_set_rtpoll(u
->sink
, i
->sink
->rtpoll
);
659 pa_sink_attach_within_thread(u
->sink
);
661 size_t fs
= pa_frame_size(&(u
->sink
->sample_spec
));
662 //pa_sink_set_latency_range_within_thread(u->sink, u->latency*fs, u->latency*fs);
663 //pa_sink_set_latency_range_within_thread(u->sink, u->latency*fs, u->master->thread_info.max_latency);
664 //TODO: setting this guy minimizes drop outs but doesn't get rid
665 //of them completely, figure out why
666 pa_sink_set_latency_range_within_thread(u
->sink
, u
->master
->thread_info
.min_latency
, u
->latency
*fs
);
667 //TODO: this guy causes dropouts constantly+rewinds, it's unusable
668 //pa_sink_set_latency_range_within_thread(u->sink, u->master->thread_info.min_latency, u->master->thread_info.max_latency);
671 /* Called from main context */
672 static void sink_input_kill_cb(pa_sink_input
*i
) {
675 pa_sink_input_assert_ref(i
);
676 pa_assert_se(u
= i
->userdata
);
678 pa_sink_unlink(u
->sink
);
679 pa_sink_input_unlink(u
->sink_input
);
681 pa_sink_unref(u
->sink
);
683 pa_sink_input_unref(u
->sink_input
);
684 u
->sink_input
= NULL
;
686 pa_module_unload_request(u
->module
, TRUE
);
689 /* Called from IO thread context */
690 static void sink_input_state_change_cb(pa_sink_input
*i
, pa_sink_input_state_t state
) {
693 pa_sink_input_assert_ref(i
);
694 pa_assert_se(u
= i
->userdata
);
696 /* If we are added for the first time, ask for a rewinding so that
697 * we are heard right-away. */
698 if (PA_SINK_INPUT_IS_LINKED(state
) &&
699 i
->thread_info
.state
== PA_SINK_INPUT_INIT
) {
700 pa_log_debug("Requesting rewind due to state change.");
701 pa_sink_input_request_rewind(i
, 0, FALSE
, TRUE
, TRUE
);
705 /* Called from main context */
706 static pa_bool_t
sink_input_may_move_to_cb(pa_sink_input
*i
, pa_sink
*dest
) {
709 pa_sink_input_assert_ref(i
);
710 pa_assert_se(u
= i
->userdata
);
712 return u
->sink
!= dest
;
716 //ensure's memory allocated is a multiple of v_size
718 static void * alloc(size_t x
,size_t s
){
719 size_t f
= mround(x
*s
, sizeof(float)*v_size
);
720 pa_assert_se(f
>= x
*s
);
721 //printf("requested %ld floats=%ld bytes, rem=%ld\n", x, x*sizeof(float), x*sizeof(float)%16);
722 //printf("giving %ld floats=%ld bytes, rem=%ld\n", f, f*sizeof(float), f*sizeof(float)%16);
723 float *t
= fftwf_malloc(f
);
728 int pa__init(pa_module
*m
) {
735 pa_sink_input_new_data sink_input_data
;
736 pa_sink_new_data sink_data
;
737 pa_bool_t
*use_default
= NULL
;
742 if (!(ma
= pa_modargs_new(m
->argument
, valid_modargs
))) {
743 pa_log("Failed to parse module arguments.");
747 if (!(master
= pa_namereg_get(m
->core
, pa_modargs_get_value(ma
, "master", NULL
), PA_NAMEREG_SINK
))) {
748 pa_log("Master sink not found");
752 ss
= master
->sample_spec
;
753 ss
.format
= PA_SAMPLE_FLOAT32
;
754 map
= master
->channel_map
;
755 if (pa_modargs_get_sample_spec_and_channel_map(ma
, &ss
, &map
, PA_CHANNEL_MAP_DEFAULT
) < 0) {
756 pa_log("Invalid sample format specification or channel map");
759 fs
= pa_frame_size(&ss
);
761 u
= pa_xnew0(struct userdata
, 1);
767 u
->sink_input
= NULL
;
769 u
->channels
= ss
.channels
;
770 u
->fft_size
= pow(2, ceil(log(ss
.rate
)/log(2)));
771 pa_log("fft size: %ld", u
->fft_size
);
772 u
->window_size
= 7999;
773 u
->R
= (u
->window_size
+1)/2;
774 u
->overlap_size
= u
->window_size
-u
->R
;
775 u
->target_samples
= 1*u
->R
;
776 u
->samples_gathered
= 0;
777 u
->max_output
= pa_frame_align(pa_mempool_block_size_max(m
->core
->mempool
), &ss
)/pa_frame_size(&ss
);
778 u
->rendered_q
= pa_memblockq_new(0, MEMBLOCKQ_MAXLENGTH
, u
->target_samples
*fs
, fs
, fs
, 0, 0, NULL
);
779 u
->a_H
= pa_aupdate_new();
780 u
->conv_buffer
.memblock
= pa_memblock_new(u
->core
->mempool
, u
->target_samples
*fs
);
782 for(size_t i
= 0; i
< 2; ++i
){
783 u
->Hs
[i
] = alloc((u
->fft_size
/ 2 + 1), sizeof(float));
785 u
->W
= alloc(u
->window_size
, sizeof(float));
786 u
->work_buffer
= alloc(u
->fft_size
, sizeof(float));
787 memset(u
->work_buffer
, 0, u
->fft_size
*sizeof(float));
788 u
->input
= (float **)pa_xmalloc0(sizeof(float *)*u
->channels
);
789 u
->overlap_accum
= (float **)pa_xmalloc0(sizeof(float *)*u
->channels
);
790 for(size_t c
= 0; c
< u
->channels
; ++c
){
791 u
->input
[c
] = alloc(u
->overlap_size
+u
->target_samples
, sizeof(float));
792 pa_assert_se(u
->input
[c
]);
793 memset(u
->input
[c
], 0, (u
->overlap_size
+u
->target_samples
)*sizeof(float));
794 pa_assert_se(u
->input
[c
]);
795 u
->overlap_accum
[c
] = alloc(u
->overlap_size
, sizeof(float));
796 pa_assert_se(u
->overlap_accum
[c
]);
797 memset(u
->overlap_accum
[c
], 0, u
->overlap_size
*sizeof(float));
799 u
->output_window
= alloc((u
->fft_size
/ 2 + 1), sizeof(fftwf_complex
));
800 u
->forward_plan
= fftwf_plan_dft_r2c_1d(u
->fft_size
, u
->work_buffer
, u
->output_window
, FFTW_MEASURE
);
801 u
->inverse_plan
= fftwf_plan_dft_c2r_1d(u
->fft_size
, u
->output_window
, u
->work_buffer
, FFTW_MEASURE
);
803 hanning_window(u
->W
, u
->window_size
);
805 unsigned H_i
= pa_aupdate_write_begin(u
->a_H
);
807 for(size_t i
= 0; i
< u
->fft_size
/ 2 + 1; ++i
){
811 //TODO cut this out and leave it for the client side
812 //const int freqs[] = {0,25,50,100,200,300,400,800,1500,
813 // 2000,3000,4000,5000,6000,7000,8000,9000,10000,11000,12000,
814 // 13000,14000,15000,16000,17000,18000,19000,20000,21000,22000,23000,24000,INT_MAX};
815 //const float coefficients[] = {1,1,1,1,1,1,1,1,1,1,
817 // 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
818 //const size_t ncoefficients = sizeof(coefficients)/sizeof(float);
819 //pa_assert_se(sizeof(freqs)/sizeof(int)==sizeof(coefficients)/sizeof(float));
820 //float *freq_translated = (float *) pa_xmalloc0(sizeof(float)*(ncoefficients));
821 //freq_translated[0] = 1;
822 ////Translate the frequencies in their natural sampling rate to the new sampling rate frequencies
823 //for(size_t i = 1; i < ncoefficients-1; ++i){
824 // freq_translated[i] = ((float)freqs[i]*u->fft_size)/ss.rate;
825 // //pa_log("i: %ld: %d , %g",i, freqs[i], freq_translated[i]);
826 // pa_assert_se(freq_translated[i] >= freq_translated[i-1]);
828 //freq_translated[ncoefficients-1] = FLT_MAX;
830 ////Interpolate the specified frequency band values
832 //for(size_t i = 1, j = 0; i < (u->fft_size / 2 + 1); ++i){
833 // pa_assert_se(j < ncoefficients);
834 // //max frequency range passed, consider the rest as one band
835 // if(freq_translated[j+1] >= FLT_MAX){
836 // for(; i < (u->fft_size / 2 + 1); ++i){
837 // u->H[i] = coefficients[j];
841 // //pa_log("i: %d, j: %d, freq: %f", i, j, freq_translated[j]);
842 // //pa_log("interp: %0.4f %0.4f", freq_translated[j], freq_translated[j+1]);
843 // pa_assert_se(freq_translated[j] < freq_translated[j+1]);
844 // pa_assert_se(i >= freq_translated[j]);
845 // pa_assert_se(i <= freq_translated[j+1]);
846 // //bilinear-inerpolation of coefficients specified
847 // float c0 = (i-freq_translated[j])/(freq_translated[j+1]-freq_translated[j]);
848 // pa_assert_se(c0 >= 0&&c0 <= 1.0);
849 // u->H[i] = ((1.0f-c0)*coefficients[j]+c0*coefficients[j+1]);
850 // pa_assert_se(u->H[i]>0);
851 // while(i >= floor(freq_translated[j+1])){
855 //pa_xfree(freq_translated);
856 fix_filter(u
->H
, u
->fft_size
);
857 pa_aupdate_write_swap(u
->a_H
);
858 pa_aupdate_write_end(u
->a_H
);
862 pa_sink_new_data_init(&sink_data
);
863 sink_data
.driver
= __FILE__
;
864 sink_data
.module
= m
;
865 if (!(sink_data
.name
= pa_xstrdup(pa_modargs_get_value(ma
, "sink_name", NULL
))))
866 sink_data
.name
= pa_sprintf_malloc("%s.equalizer", master
->name
);
867 sink_data
.namereg_fail
= FALSE
;
868 pa_sink_new_data_set_sample_spec(&sink_data
, &ss
);
869 pa_sink_new_data_set_channel_map(&sink_data
, &map
);
870 z
= pa_proplist_gets(master
->proplist
, PA_PROP_DEVICE_DESCRIPTION
);
871 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_DESCRIPTION
, "FFT based equalizer");
872 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_MASTER_DEVICE
, master
->name
);
873 pa_proplist_sets(sink_data
.proplist
, PA_PROP_DEVICE_CLASS
, "filter");
875 if (pa_modargs_get_proplist(ma
, "sink_properties", sink_data
.proplist
, PA_UPDATE_REPLACE
) < 0) {
876 pa_log("Invalid properties");
877 pa_sink_new_data_done(&sink_data
);
881 u
->sink
= pa_sink_new(m
->core
, &sink_data
, PA_SINK_LATENCY
|PA_SINK_DYNAMIC_LATENCY
);
882 pa_sink_new_data_done(&sink_data
);
885 pa_log("Failed to create sink.");
889 u
->sink
->parent
.process_msg
= sink_process_msg
;
890 u
->sink
->set_state
= sink_set_state
;
891 u
->sink
->update_requested_latency
= sink_update_requested_latency
;
892 u
->sink
->request_rewind
= sink_request_rewind
;
893 u
->sink
->userdata
= u
;
895 pa_sink_set_asyncmsgq(u
->sink
, master
->asyncmsgq
);
896 pa_sink_set_rtpoll(u
->sink
, master
->rtpoll
);
897 pa_sink_set_max_request(u
->sink
, u
->R
*fs
);
898 //pa_sink_set_fixed_latency(u->sink, pa_bytes_to_usec(u->R*fs, &ss));
900 /* Create sink input */
901 pa_sink_input_new_data_init(&sink_input_data
);
902 sink_input_data
.driver
= __FILE__
;
903 sink_input_data
.module
= m
;
904 sink_input_data
.sink
= u
->master
;
905 pa_proplist_sets(sink_input_data
.proplist
, PA_PROP_MEDIA_NAME
, "Equalized Stream");
906 pa_proplist_sets(sink_input_data
.proplist
, PA_PROP_MEDIA_ROLE
, "filter");
907 pa_sink_input_new_data_set_sample_spec(&sink_input_data
, &ss
);
908 pa_sink_input_new_data_set_channel_map(&sink_input_data
, &map
);
910 pa_sink_input_new(&u
->sink_input
, m
->core
, &sink_input_data
, PA_SINK_INPUT_DONT_MOVE
);
911 pa_sink_input_new_data_done(&sink_input_data
);
916 u
->sink_input
->pop
= sink_input_pop_cb
;
917 u
->sink_input
->process_rewind
= sink_input_process_rewind_cb
;
918 u
->sink_input
->update_max_rewind
= sink_input_update_max_rewind_cb
;
919 u
->sink_input
->update_max_request
= sink_input_update_max_request_cb
;
920 u
->sink_input
->update_sink_latency_range
= sink_input_update_sink_latency_range_cb
;
921 u
->sink_input
->kill
= sink_input_kill_cb
;
922 u
->sink_input
->attach
= sink_input_attach_cb
;
923 u
->sink_input
->detach
= sink_input_detach_cb
;
924 u
->sink_input
->state_change
= sink_input_state_change_cb
;
925 u
->sink_input
->may_move_to
= sink_input_may_move_to_cb
;
926 u
->sink_input
->userdata
= u
;
928 pa_sink_put(u
->sink
);
929 pa_sink_input_put(u
->sink_input
);
933 pa_xfree(use_default
);
943 pa_xfree(use_default
);
950 int pa__get_n_used(pa_module
*m
) {
954 pa_assert_se(u
= m
->userdata
);
956 return pa_sink_linked_by(u
->sink
);
959 void pa__done(pa_module
*m
) {
964 if (!(u
= m
->userdata
))
969 pa_sink_unlink(u
->sink
);
970 pa_sink_unref(u
->sink
);
974 pa_sink_input_unlink(u
->sink_input
);
975 pa_sink_input_unref(u
->sink_input
);
978 if(u
->conv_buffer
.memblock
)
979 pa_memblock_unref(u
->conv_buffer
.memblock
);
982 pa_memblockq_free(u
->rendered_q
);
984 fftwf_destroy_plan(u
->inverse_plan
);
985 fftwf_destroy_plan(u
->forward_plan
);
986 pa_xfree(u
->output_window
);
987 for(size_t c
=0; c
< u
->channels
; ++c
){
988 pa_xfree(u
->overlap_accum
[c
]);
989 pa_xfree(u
->input
[c
]);
991 pa_xfree(u
->overlap_accum
);
993 pa_xfree(u
->work_buffer
);
995 for(size_t i
= 0; i
< 2; ++i
){
1002 enum property_handler_index
{
1003 PROPERTY_HANDLER_N_COEFS
,
1004 PROPERTY_HANDLER_COEFS
,
1005 PROPERTY_HANDLER_MAX
1008 static pa_dbus_property_handler property_handlers
[PROPERTY_HANDLER_MAX
]={
1009 [PROPERTY_HANDLER_N_COEFS
]{.property_name
="n_filter_coefficients",.type
="u",.get_cb
=get_n_coefs
,.set_cb
=NULL
},
1010 [PROPERTY_HANDLER_COEFS
]{.property_name
="filter_coefficients",.type
="ai",.get_cb
=get_filter
,.set_cb
=set_filter
}
1013 //static pa_dbus_arg_info new_equalizer_args[] = { { "path","o",NULL} };
1014 //static pa_dbus_signal_info signals[SIGNAL_MAX] = {
1015 // [SIGNAL_NEW_EQUALIZER]={.name="NewEqualizer",.arguments=new_equalizer_args,.n_arguments=1}
1018 #define EXTNAME "org.PulseAudio.Ext.Equalizing1"
1020 static pa_dbus_interface_info interface_info
={
1021 .name
=EXTNAME
".Equalizer",
1022 .method_handlers
=NULL
,
1023 .n_method_handlers
=0,
1024 .property_handlers
=property_handlers
,
1025 .n_property_handlers
=PROPERTY_HANDLER_MAX
,
1026 .get_all_properties_cb
=handle_get_all
,
1032 void dbus_init(struct userdata
*u
){
1033 u
->dbus_protocol
=pa_dbus_protocol_get(u
->core
);
1034 u
->dbus_path
=pa_sprintf_malloc("/org/pulseaudio/core1/sink%d", u
->sink
->index
);
1036 pa_dbus_protocol_add_interface(u
->dbus_protocol
, u
->dbus_path
, &interface_info
, u
);
1037 pa_dbus_protocol_register_extension(u
->dbus_protocol
, EXTNAME
);
1040 void dbus_done(struct userdata
*u
){
1041 pa_dbus_protocol_unregister_extension(u
->dbus_protocol
, EXTNAME
);
1042 pa_dbus_protocol_remove_interface(u
->dbus_protocol
, u
->dbus_path
, EXTNAME
);
1044 pa_xfree(u
->dbus_path
);
1045 pa_dbus_protocol_unref(u
->dbus_protocol
);
1048 void get_n_coefs(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
){
1053 struct userdata
*u
=(struct userdata
*)_u
;
1055 uint32_t n_coefs
=(uint32_t)(u
->fft_size
/ 2 + 1);
1056 pa_dbus_send_basic_variant_reply(conn
, msg
, DBUS_TYPE_UINT32
, &n_coefs
);
1059 void get_filter(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
){
1064 struct userdata
*u
=(struct userdata
*)_u
;
1066 unsigned n_coefs
=(unsigned)(u
->fft_size
/ 2 + 1);
1067 double *H_
=(double *)pa_xmalloc0(n_coefs
*sizeof(double));
1069 unsigned H_i
=pa_aupdate_read_begin(u
->a_H
);
1070 float *H
=u
->Hs
[H_i
];
1071 for(size_t i
= 0;i
< u
->fft_size
/ 2 + 1; ++i
){
1074 pa_aupdate_read_end(u
->a_H
);
1075 pa_dbus_send_basic_array_variant_reply(conn
, msg
, DBUS_TYPE_DOUBLE
, &H_
, n_coefs
);
1079 void set_filter(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
){
1084 struct userdata
*u
=(struct userdata
*)_u
;
1087 pa_dbus_get_fixed_array_set_property_arg(conn
, msg
, DBUS_TYPE_DOUBLE
, &H_
, &_n_coefs
);
1088 if(_n_coefs
!=u
->fft_size
/ 2 + 1){
1089 pa_dbus_send_error(conn
, msg
, DBUS_ERROR_INVALID_ARGS
, "This filter takes exactly %ld coefficients, you gave %d", u
->fft_size
/ 2 + 1, _n_coefs
);
1092 unsigned H_i
= pa_aupdate_write_begin(u
->a_H
);
1093 float *H
= u
->Hs
[H_i
];
1094 for(size_t i
= 0; i
< u
->fft_size
/ 2 + 1; ++i
){
1095 H
[i
] = (float)H_
[i
];
1097 pa_aupdate_write_swap(u
->a_H
);
1098 pa_aupdate_write_end(u
->a_H
);
1100 pa_dbus_send_empty_reply(conn
, msg
);
1103 void handle_get_all(DBusConnection
*conn
, DBusMessage
*msg
, void *_u
){
1108 struct userdata
*u
= (struct userdata
*)_u
;
1109 DBusMessage
*reply
= NULL
;
1110 DBusMessageIter msg_iter
, dict_iter
;
1112 int n_coefs
=(unsigned)(u
->fft_size
/ 2 + 1);
1113 double *H_
=(double *)pa_xmalloc0(n_coefs
*sizeof(double));
1115 unsigned H_i
=pa_aupdate_read_begin(u
->a_H
);
1116 float *H
=u
->Hs
[H_i
];
1117 for(size_t i
= 0; i
< u
->fft_size
/ 2 + 1; ++i
){
1120 pa_aupdate_read_end(u
->a_H
);
1122 pa_assert_se((reply
= dbus_message_new_method_return(msg
)));
1123 dbus_message_iter_init_append(reply
, &msg_iter
);
1124 pa_assert_se(dbus_message_iter_open_container(&msg_iter
, DBUS_TYPE_ARRAY
, "{sv}", &dict_iter
));
1126 pa_dbus_append_basic_variant_dict_entry(&dict_iter
, property_handlers
[PROPERTY_HANDLER_N_COEFS
].property_name
, DBUS_TYPE_UINT32
, &n_coefs
);
1127 pa_dbus_append_basic_array_variant_dict_entry(&dict_iter
, property_handlers
[PROPERTY_HANDLER_COEFS
].property_name
, DBUS_TYPE_DOUBLE
, H_
, n_coefs
);
1129 pa_assert_se(dbus_message_iter_close_container(&msg_iter
, &dict_iter
));
1130 pa_assert_se(dbus_connection_send(conn
, reply
, NULL
));
1131 dbus_message_unref(reply
);