src/modules/module-equalizer-sink.c

   1 /***
   2 This file is part of PulseAudio.
   3
   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>
   8
   9 Original Author:
  10 Copyright 2004-2008 Lennart Poettering
  11
  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.
  16
  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.
  21
  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
  25 USA.
  26 ***/
  27
  28 #ifdef HAVE_CONFIG_H
  29 #include <config.h>
  30 #endif
  31
  32 #include <stdio.h>
  33 #include <math.h>
  34 #include <fftw3.h>
  35 #include <float.h>
  36
  37
  38 #include <pulse/xmalloc.h>
  39 #include <pulse/i18n.h>
  40
  41 #include <pulsecore/core-error.h>
  42 #include <pulsecore/namereg.h>
  43 #include <pulsecore/sink.h>
  44 #include <pulsecore/module.h>
  45 #include <pulsecore/core-util.h>
  46 #include <pulsecore/modargs.h>
  47 #include <pulsecore/log.h>
  48 #include <pulsecore/thread.h>
  49 #include <pulsecore/thread-mq.h>
  50 #include <pulsecore/rtpoll.h>
  51 #include <pulsecore/sample-util.h>
  52 #include <pulsecore/ltdl-helper.h>
  53
  54 #include <stdint.h>
  55 #include <time.h>
  56
  57
  58 #include "module-equalizer-sink-symdef.h"
  59
  60 PA_MODULE_AUTHOR("Jason Newton");
  61 PA_MODULE_DESCRIPTION(_("General Purpose Equalizer"));
  62 PA_MODULE_VERSION(PACKAGE_VERSION);
  63 PA_MODULE_LOAD_ONCE(FALSE);
  64 PA_MODULE_USAGE(_("sink=<sink to connect to> "));
  65
  66 #define MEMBLOCKQ_MAXLENGTH (16*1024*1024)
  67
  68 struct userdata {
  69     pa_core *core;
  70     pa_module *module;
  71     pa_sink *sink, *master;
  72     pa_sink_input *sink_input;
  73
  74     size_t channels;
  75     size_t fft_size;//length (res) of fft
  76     size_t window_size;/*
  77                         *sliding window size
  78                         *effectively chooses R
  79                         */
  80     size_t R;/* the hop size between overlapping windows
  81               * the latency of the filter, calculated from window_size
  82               * based on constraints of COLA and window function
  83               */
  84     size_t overlap_size;//window_size-R
  85     size_t samples_gathered;
  86     size_t max_output;
  87     size_t target_samples;
  88     float *H;//frequency response filter (magnitude based)
  89     float *W;//windowing function (time domain)
  90     float *work_buffer,**input,**overlap_accum,**output_buffer;
  91     fftwf_complex *output_window;
  92     fftwf_plan forward_plan,inverse_plan;
  93     //size_t samplings;
  94
  95     pa_memchunk conv_buffer;
  96     pa_memblockq *rendered_q;
  97 };
  98
  99 static const char* const valid_modargs[] = {
 100     "sink_name",
 101     "sink_properties",
 102     "master",
 103     "format",
 104     "rate",
 105     "channels",
 106     "channel_map",
 107     NULL
 108 };
 109
 110 uint64_t time_diff(struct timespec *timeA_p, struct timespec *timeB_p)
 111 {
 112     return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) -
 113     ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec);
 114 }
 115
 116 void hanning_normalized_window(float *W,size_t window_size){
 117     //h = sqrt(2)/2 * (1+cos(t*pi)) ./ sqrt( 1+cos(t*pi).^2 )
 118     float c;
 119     for(size_t i=0;i<window_size;++i){
 120         c=cos(M_PI*i/(window_size-1));
 121         W[i]=sqrt(2.0)/2.0*(1.0+c) / sqrt(1.0+c*c);
 122     }
 123 }
 124 void hanning_window(float *W,size_t window_size){
 125     //h=.5*(1-cos(2*pi*j/(window_size+1)), COLA for R=(M+1)/2
 126     for(size_t i=0;i<window_size;++i){
 127         W[i]=.5*(1-cos(2*M_PI*i/(window_size+1)));
 128     }
 129 }
 130 void hamming_window(float *W,size_t window_size){
 131     //h=.54-.46*cos(2*pi*j/(window_size-1))
 132     //COLA for R=(M-1)/2,(M-1)/4 etc when endpoints are divided by 2
 133     //or one endpoint is zeroed
 134     float m;
 135     for(size_t i=0;i<window_size;++i){
 136         m=i;
 137         m/=(window_size-1);
 138         W[i]=.54-.46*cos(2*M_PI*m);
 139     }
 140     W[window_size-1]=0;
 141     //W[0]/=2;
 142     //W[window_size-1]/=2;
 143 }
 144 void blackman_window(float *W,size_t window_size){
 145     //h=.42-.5*cos(2*pi*m)+.08*cos(4*pi*m), m=(0:W-1)/(W-1)
 146     //COLA for R=(M-1)/3 when M is odd and R is an integer
 147     //R=M/3 when M is even and R is an integer
 148     float m;
 149     for(size_t i=0;i<window_size;++i){
 150         m=i;
 151         m/=(window_size-1);
 152         W[i]=.42-.5*cos(2*M_PI*m)+.08*cos(4*M_PI*m);
 153     }
 154 }
 155
 156
 157 void sin_window(float *W,size_t window_size){
 158     //h = (cos(t*pi)+1)/2 .* float(abs(t)<1);
 159     for(size_t i=0;i<window_size;++i){
 160         W[i]=sin(M_PI*i/(window_size-1));
 161     }
 162 }
 163
 164
 165 void array_out(const char *name,float *a,size_t length){
 166     FILE *p=fopen(name,"w");
 167     if(!p){
 168         pa_log("opening %s failed!",name);
 169         return;
 170     }
 171     for(size_t i=0;i<length;++i){
 172         fprintf(p,"%e,",a[i]);
 173         //if(i%1000==0){
 174         //    fprintf(p,"\n");
 175         //}
 176     }
 177     fprintf(p,"\n");
 178     fclose(p);
 179 }
 180
 181
 182 /* Called from I/O thread context */
 183 static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offset, pa_memchunk *chunk) {
 184     struct userdata *u = PA_SINK(o)->userdata;
 185
 186     switch (code) {
 187
 188         case PA_SINK_MESSAGE_GET_LATENCY: {
 189             pa_usec_t usec = 0;
 190             pa_sample_spec *ss=&u->sink->sample_spec;
 191             size_t fs=pa_frame_size(ss);
 192
 193             /* Get the latency of the master sink */
 194             if (PA_MSGOBJECT(u->master)->process_msg(PA_MSGOBJECT(u->master), PA_SINK_MESSAGE_GET_LATENCY, &usec, 0, NULL) < 0)
 195                 usec = 0;
 196
 197             usec+=pa_bytes_to_usec(u->samples_gathered*fs,ss);
 198             usec += pa_bytes_to_usec(pa_memblockq_get_length(u->rendered_q), ss);
 199             /* Add the latency internal to our sink input on top */
 200             usec += pa_bytes_to_usec(pa_memblockq_get_length(u->sink_input->thread_info.render_memblockq), &u->master->sample_spec);
 201             *((pa_usec_t*) data) = usec;
 202             return 0;
 203         }
 204     }
 205
 206     return pa_sink_process_msg(o, code, data, offset, chunk);
 207 }
 208
 209
 210 /* Called from main context */
 211 static int sink_set_state(pa_sink *s, pa_sink_state_t state) {
 212     struct userdata *u;
 213
 214     pa_sink_assert_ref(s);
 215     pa_assert_se(u = s->userdata);
 216
 217     if (PA_SINK_IS_LINKED(state) &&
 218         u->sink_input &&
 219         PA_SINK_INPUT_IS_LINKED(pa_sink_input_get_state(u->sink_input)))
 220
 221         pa_sink_input_cork(u->sink_input, state == PA_SINK_SUSPENDED);
 222
 223     return 0;
 224 }
 225
 226 /* Called from I/O thread context */
 227 static void sink_request_rewind(pa_sink *s) {
 228     struct userdata *u;
 229
 230     pa_sink_assert_ref(s);
 231     pa_assert_se(u = s->userdata);
 232
 233     /* Just hand this one over to the master sink */
 234     pa_sink_input_request_rewind(u->sink_input, s->thread_info.rewind_nbytes + pa_memblockq_get_length(u->rendered_q), TRUE, FALSE, FALSE);
 235 }
 236
 237 /* Called from I/O thread context */
 238 static void sink_update_requested_latency(pa_sink *s) {
 239     struct userdata *u;
 240
 241     pa_sink_assert_ref(s);
 242     pa_assert_se(u = s->userdata);
 243
 244     /* Just hand this one over to the master sink */
 245     pa_sink_input_set_requested_latency_within_thread(
 246             u->sink_input,
 247             pa_sink_get_requested_latency_within_thread(s));
 248 }
 249
 250 /* Called from I/O thread context */
 251 static int sink_input_pop_cb(pa_sink_input *i, size_t nbytes, pa_memchunk *chunk) {
 252     struct userdata *u;
 253     pa_sink_input_assert_ref(i);
 254     pa_assert(chunk);
 255     pa_assert_se(u = i->userdata);
 256     pa_assert_se(u->sink);
 257     size_t fs=pa_frame_size(&(u->sink->sample_spec));
 258     size_t ss=pa_sample_size(&(u->sink->sample_spec));
 259     size_t fe = fs/ss;
 260     size_t samples_requested=nbytes/fs;
 261     pa_memchunk tchunk;
 262     chunk->memblock=NULL;
 263     size_t buffered_samples=pa_memblockq_get_length(u->rendered_q)/fs;
 264
 265     if (!u->sink || !PA_SINK_IS_OPENED(u->sink->thread_info.state))
 266         return -1;
 267
 268     pa_log("start output-buffered %ld, input-buffered %ld",buffered_samples,u->samples_gathered);
 269     //collect samples
 270     size_t buffered_remaining=pa_memblockq_get_length(u->rendered_q)/fs;
 271     size_t buffer_missing=pa_memblockq_missing(u->rendered_q)/fs;
 272     size_t desired_samples=(buffer_missing>=u->R)*PA_MIN(u->target_samples-u->samples_gathered,buffer_missing);
 273     if(desired_samples>0){
 274         u->conv_buffer.index=0;
 275         //if we still had buffered output,
 276         //or can gather any more in the buffer
 277         //politely request (optimistic)
 278         if(buffered_samples>=samples_requested ||
 279             (u->samples_gathered/u->R)*u->R>=samples_requested){
 280             u->conv_buffer.length=desired_samples*fs;
 281             pa_log("trying to buffer %ld samples",desired_samples);
 282             pa_sink_render_into(u->sink, &u->conv_buffer);
 283         }else{//we need it now! force it
 284             //TODO: minimum amount or the whole buffer better?
 285             desired_samples=u->R-u->samples_gathered%u->R;
 286             u->conv_buffer.length=desired_samples*fs;
 287             pa_log("force-buffer %ld samples",desired_samples);
 288             pa_sink_render_into_full(u->sink, &u->conv_buffer);
 289             pa_assert_se(u->conv_buffer.length==desired_samples*fs);
 290         }
 291         size_t n_samples=u->conv_buffer.length/fs;
 292         float *src;
 293         pa_log("received %ld samples",n_samples);
 294
 295         pa_assert_se(n_samples<=u->target_samples-u->samples_gathered);
 296         src = (float*) ((uint8_t*) pa_memblock_acquire(u->conv_buffer.memblock) + u->conv_buffer.index);
 297         for (size_t c=0;c<u->channels;c++) {
 298             //buffer with an offset after the overlap from previous
 299             //iterations
 300             pa_assert_se(
 301                 u->input[c]+u->overlap_size+u->samples_gathered+n_samples<=u->input[c]+u->target_samples+u->overlap_size
 302             );
 303             pa_sample_clamp(PA_SAMPLE_FLOAT32NE,u->input[c]+u->overlap_size+u->samples_gathered,sizeof(float), src+c, fs, n_samples);
 304         }
 305         u->samples_gathered+=n_samples;
 306         pa_memblock_release(u->conv_buffer.memblock);
 307     }
 308     //pa_assert_se(u->samples_gathered>=u->R);
 309     pa_assert_se(u->fft_size>=u->window_size);
 310     pa_assert_se(u->R<u->window_size);
 311     //process every complete block on hand
 312     while(u->samples_gathered>=u->R&&buffer_missing>=u->R){
 313         float *dst;
 314         //pa_log("iter gathered: %ld",u->samples_gathered);
 315         tchunk.index=0;
 316         tchunk.length=u->R*fs;
 317         tchunk.memblock=pa_memblock_new(u->core->mempool,tchunk.length);
 318         //pa_memblockq_drop(u->rendered_q, tchunk.length);
 319         pa_assert_se(tchunk.length==u->R*fs);
 320         dst=(float*)pa_memblock_acquire(tchunk.memblock);
 321         //use a linear-phase sliding STFT and overlap-add method (for each channel)
 322         for (size_t c=0;c<u->channels;c++) {
 323             //zero padd the data
 324             memset(u->work_buffer+u->window_size,0,(u->fft_size-u->window_size)*sizeof(float));
 325             //window the data
 326             for(size_t j=0;j<u->window_size;++j){
 327                 u->work_buffer[j]=u->W[j]*u->input[c][j];
 328             }
 329             //Processing is done here!
 330             //do fft
 331             fftwf_execute_dft_r2c(u->forward_plan,u->work_buffer,u->output_window);
 332             //perform filtering
 333             for(size_t j=0;j<u->fft_size/2+1;++j){
 334                 u->output_window[j][0]*=u->H[j];
 335                 u->output_window[j][1]*=u->H[j];
 336             }
 337             //inverse fft
 338             fftwf_execute_dft_c2r(u->inverse_plan,u->output_window,u->work_buffer);
 339             ////debug: tests overlaping add
 340             ////and negates ALL PREVIOUS processing
 341             ////yields a perfect reconstruction if COLA is held
 342             //for(size_t j=0;j<u->window_size;++j){
 343             //    u->work_buffer[j]=u->W[j]*u->input[c][j];
 344             //}
 345
 346             //overlap add and preserve overlap component from this window (linear phase)
 347             for(size_t j=0;j<u->R;++j){
 348                 u->work_buffer[j]+=u->overlap_accum[c][j];
 349                 u->overlap_accum[c][j]=u->work_buffer[u->overlap_size+j];
 350             }
 351
 352             //debug: tests if basic buffering works
 353             //shouldn't modify the signal AT ALL (beyond roundoff)
 354             for(size_t j=0;j<u->window_size;++j){
 355                 u->work_buffer[j]=u->input[c][j];
 356             }
 357
 358             //preseve the needed input for the next window's overlap
 359             memmove(u->input[c],u->input[c]+u->R,
 360                 (u->samples_gathered+u->overlap_size-u->R)*sizeof(float)
 361             );
 362             //output the samples that are outputable now
 363             pa_sample_clamp(PA_SAMPLE_FLOAT32NE, dst+c, fs, u->work_buffer, sizeof(float),u->R);
 364         }
 365         pa_memblock_release(tchunk.memblock);
 366         pa_memblockq_push(u->rendered_q, &tchunk);
 367         pa_memblock_unref(tchunk.memblock);
 368         u->samples_gathered-=u->R;
 369         buffer_missing-=u->R;
 370     }
 371     //deque from renderq and output
 372     //pa_memblockq_set_prebuf(u->rendered_q,samples_requested*fs);
 373     pa_assert_se(pa_memblockq_peek(u->rendered_q,&tchunk)>=0);
 374     if(tchunk.length>=nbytes){
 375         *chunk=tchunk;
 376         chunk->length=samples_requested*fs;
 377         pa_memblock_ref(chunk->memblock);
 378         pa_memblock_unref(tchunk.memblock);
 379         pa_memblockq_drop(u->rendered_q, chunk->length);
 380     }else{
 381         size_t copied=0;
 382         chunk->length=nbytes;
 383         chunk->memblock=pa_memblock_new(u->core->mempool,chunk->length);
 384         uint8_t *dst=(uint8_t*)pa_memblock_acquire(chunk->memblock);
 385         do{
 386             size_t l=PA_MIN(tchunk.length-tchunk.index,nbytes-copied);
 387             uint8_t *src=(((uint8_t*)pa_memblock_acquire(tchunk.memblock))+tchunk.index);
 388             memmove(dst+copied,src,l);
 389             copied+=l;
 390             pa_memblock_release(tchunk.memblock);
 391             pa_memblock_unref(tchunk.memblock);
 392             pa_memblockq_drop(u->rendered_q,l);
 393             if(copied<nbytes){
 394                 if(pa_memblockq_get_length(u->rendered_q)==0){
 395                     chunk->length=copied;
 396                     break;
 397                 }
 398                 pa_memblockq_peek(u->rendered_q,&tchunk);
 399             }
 400         }while(copied<nbytes);
 401         pa_memblock_release(chunk->memblock);
 402     }
 403     pa_assert_se(chunk->memblock);
 404     pa_log("output requested %ld, gave %ld",nbytes/fs,chunk->length/fs);
 405     //pa_log("end pop");
 406     return 0;
 407 }
 408
 409 /* Called from I/O thread context */
 410 static void sink_input_process_rewind_cb(pa_sink_input *i, size_t nbytes) {
 411     struct userdata *u;
 412     size_t amount = 0;
 413
 414     pa_sink_input_assert_ref(i);
 415     pa_assert_se(u = i->userdata);
 416
 417     if (!u->sink || !PA_SINK_IS_OPENED(u->sink->thread_info.state))
 418         return;
 419
 420     if (u->sink->thread_info.rewind_nbytes > 0) {
 421         size_t max_rewrite;
 422
 423         max_rewrite = nbytes + pa_memblockq_get_length(u->rendered_q);
 424         amount = PA_MIN(u->sink->thread_info.rewind_nbytes, max_rewrite);
 425         u->sink->thread_info.rewind_nbytes = 0;
 426
 427         if (amount > 0) {
 428             pa_memblockq_seek(u->rendered_q, - (int64_t) amount, PA_SEEK_RELATIVE, TRUE);
 429             pa_log_debug("Resetting equalizer");
 430             u->samples_gathered=0;
 431         }
 432     }
 433
 434     pa_sink_process_rewind(u->sink, amount);
 435     pa_memblockq_rewind(u->rendered_q, nbytes);
 436 }
 437
 438 /* Called from I/O thread context */
 439 static void sink_input_update_max_rewind_cb(pa_sink_input *i, size_t nbytes) {
 440     struct userdata *u;
 441
 442     pa_sink_input_assert_ref(i);
 443     pa_assert_se(u = i->userdata);
 444
 445     if (!u->sink || !PA_SINK_IS_LINKED(u->sink->thread_info.state))
 446         return;
 447
 448     pa_memblockq_set_maxrewind(u->rendered_q, nbytes);
 449     pa_sink_set_max_rewind_within_thread(u->sink, nbytes);
 450 }
 451
 452 /* Called from I/O thread context */
 453 static void sink_input_update_max_request_cb(pa_sink_input *i, size_t nbytes) {
 454     struct userdata *u;
 455
 456     pa_sink_input_assert_ref(i);
 457     pa_assert_se(u = i->userdata);
 458
 459     if (!u->sink || !PA_SINK_IS_LINKED(u->sink->thread_info.state))
 460         return;
 461
 462     pa_sink_set_max_request_within_thread(u->sink, nbytes);
 463 }
 464
 465 /* Called from I/O thread context */
 466 static void sink_input_update_sink_latency_range_cb(pa_sink_input *i) {
 467     struct userdata *u;
 468
 469     pa_sink_input_assert_ref(i);
 470     pa_assert_se(u = i->userdata);
 471
 472     if (!u->sink || !PA_SINK_IS_LINKED(u->sink->thread_info.state))
 473         return;
 474
 475     pa_sink_set_latency_range_within_thread(u->sink, i->sink->thread_info.min_latency, i->sink->thread_info.max_latency);
 476 }
 477
 478 /* Called from I/O thread context */
 479 static void sink_input_detach_cb(pa_sink_input *i) {
 480     struct userdata *u;
 481
 482     pa_sink_input_assert_ref(i);
 483     pa_assert_se(u = i->userdata);
 484
 485     if (!u->sink || !PA_SINK_IS_LINKED(u->sink->thread_info.state))
 486         return;
 487
 488     pa_sink_detach_within_thread(u->sink);
 489     pa_sink_set_asyncmsgq(u->sink, NULL);
 490     pa_sink_set_rtpoll(u->sink, NULL);
 491 }
 492
 493 /* Called from I/O thread context */
 494 static void sink_input_attach_cb(pa_sink_input *i) {
 495     struct userdata *u;
 496
 497     pa_sink_input_assert_ref(i);
 498     pa_assert_se(u = i->userdata);
 499
 500     if (!u->sink || !PA_SINK_IS_LINKED(u->sink->thread_info.state))
 501         return;
 502
 503     pa_sink_set_asyncmsgq(u->sink, i->sink->asyncmsgq);
 504     pa_sink_set_rtpoll(u->sink, i->sink->rtpoll);
 505     pa_sink_attach_within_thread(u->sink);
 506
 507     pa_sink_set_latency_range_within_thread(u->sink, u->master->thread_info.min_latency, u->master->thread_info.max_latency);
 508 }
 509
 510 /* Called from main context */
 511 static void sink_input_kill_cb(pa_sink_input *i) {
 512     struct userdata *u;
 513
 514     pa_sink_input_assert_ref(i);
 515     pa_assert_se(u = i->userdata);
 516
 517     pa_sink_unlink(u->sink);
 518     pa_sink_input_unlink(u->sink_input);
 519
 520     pa_sink_unref(u->sink);
 521     u->sink = NULL;
 522     pa_sink_input_unref(u->sink_input);
 523     u->sink_input = NULL;
 524
 525     pa_module_unload_request(u->module, TRUE);
 526 }
 527
 528 /* Called from IO thread context */
 529 static void sink_input_state_change_cb(pa_sink_input *i, pa_sink_input_state_t state) {
 530     struct userdata *u;
 531
 532     pa_sink_input_assert_ref(i);
 533     pa_assert_se(u = i->userdata);
 534
 535     /* If we are added for the first time, ask for a rewinding so that
 536      * we are heard right-away. */
 537     if (PA_SINK_INPUT_IS_LINKED(state) &&
 538         i->thread_info.state == PA_SINK_INPUT_INIT) {
 539         pa_log_debug("Requesting rewind due to state change.");
 540         pa_sink_input_request_rewind(i, 0, FALSE, TRUE, TRUE);
 541     }
 542 }
 543
 544 /* Called from main context */
 545 static pa_bool_t sink_input_may_move_to_cb(pa_sink_input *i, pa_sink *dest) {
 546     struct userdata *u;
 547
 548     pa_sink_input_assert_ref(i);
 549     pa_assert_se(u = i->userdata);
 550
 551     return u->sink != dest;
 552 }
 553
 554 int pa__init(pa_module*m) {
 555     struct userdata *u;
 556     pa_sample_spec ss;
 557     pa_channel_map map;
 558     pa_modargs *ma;
 559     const char *z;
 560     pa_sink *master;
 561     pa_sink_input_new_data sink_input_data;
 562     pa_sink_new_data sink_data;
 563     pa_bool_t *use_default = NULL;
 564     size_t fs;
 565
 566     pa_assert(m);
 567
 568     if (!(ma = pa_modargs_new(m->argument, valid_modargs))) {
 569         pa_log("Failed to parse module arguments.");
 570         goto fail;
 571     }
 572
 573     if (!(master = pa_namereg_get(m->core, pa_modargs_get_value(ma, "master", NULL), PA_NAMEREG_SINK))) {
 574         pa_log("Master sink not found");
 575         goto fail;
 576     }
 577
 578     ss = master->sample_spec;
 579     ss.format = PA_SAMPLE_FLOAT32;
 580     map = master->channel_map;
 581     if (pa_modargs_get_sample_spec_and_channel_map(ma, &ss, &map, PA_CHANNEL_MAP_DEFAULT) < 0) {
 582         pa_log("Invalid sample format specification or channel map");
 583         goto fail;
 584     }
 585     fs=pa_frame_size(&ss);
 586
 587     u = pa_xnew0(struct userdata, 1);
 588     u->core = m->core;
 589     u->module = m;
 590     m->userdata = u;
 591     u->master = master;
 592     u->sink = NULL;
 593     u->sink_input = NULL;
 594
 595     u->channels=ss.channels;
 596     u->fft_size=pow(2,ceil(log(ss.rate)/log(2)));
 597     pa_log("fft size: %ld",u->fft_size);
 598     u->window_size=7999;
 599     u->R=(u->window_size+1)/2;
 600     u->overlap_size=u->window_size-u->R;
 601     u->target_samples=5*u->R;
 602     u->samples_gathered=0;
 603     u->max_output=pa_frame_align(pa_mempool_block_size_max(m->core->mempool), &ss)/pa_frame_size(&ss);
 604     u->rendered_q = pa_memblockq_new(0, MEMBLOCKQ_MAXLENGTH,u->target_samples*fs, fs, fs, 0, 0, NULL);
 605     u->conv_buffer.memblock=pa_memblock_new(u->core->mempool,u->target_samples*fs);
 606
 607
 608     u->H=(float*) fftwf_malloc((u->fft_size/2+1)*sizeof(float));
 609     u->W=(float*) fftwf_malloc((u->window_size)*sizeof(float));
 610     u->work_buffer=(float*) fftwf_malloc(u->fft_size*sizeof(float));
 611     u->input=(float **)malloc(sizeof(float *)*u->channels);
 612     u->overlap_accum=(float **)malloc(sizeof(float *)*u->channels);
 613     u->output_buffer=(float **)malloc(sizeof(float *)*u->channels);
 614     for(size_t c=0;c<u->channels;++c){
 615         u->input[c]=(float*) fftwf_malloc((u->target_samples+u->overlap_size)*sizeof(float));
 616         pa_assert_se(u->input[c]);
 617         memset(u->input[c],0,(u->target_samples+u->overlap_size)*sizeof(float));
 618         pa_assert_se(u->input[c]);
 619         u->overlap_accum[c]=(float*) fftwf_malloc(u->R*sizeof(float));
 620         pa_assert_se(u->overlap_accum[c]);
 621         memset(u->overlap_accum[c],0,u->R*sizeof(float));
 622         u->output_buffer[c]=(float*) fftwf_malloc(u->window_size*sizeof(float));
 623         pa_assert_se(u->output_buffer[c]);
 624     }
 625     u->output_window = (fftwf_complex *) fftwf_malloc(sizeof(fftwf_complex) * (u->fft_size/2+1));
 626     u->forward_plan=fftwf_plan_dft_r2c_1d(u->fft_size, u->work_buffer, u->output_window, FFTW_ESTIMATE);
 627     u->inverse_plan=fftwf_plan_dft_c2r_1d(u->fft_size, u->output_window, u->work_buffer, FFTW_ESTIMATE);
 628     /*
 629     for(size_t j=0;j<u->window_size;++j){
 630         u->W[j]=.5;
 631     }
 632     */
 633     hanning_window(u->W,u->window_size);
 634
 635     const int freqs[]={0,25,50,100,200,300,400,800,1500,
 636         2000,3000,4000,5000,6000,7000,8000,9000,10000,11000,12000,
 637         13000,14000,15000,16000,17000,18000,19000,20000,21000,22000,23000,24000,INT_MAX};
 638     const float coefficients[]={1,1,1,1,1,1,1,1,1,1,
 639         1,1,1,1,1,1,1,1,
 640         1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
 641     const size_t ncoefficients=sizeof(coefficients)/sizeof(float);
 642     pa_assert_se(sizeof(freqs)/sizeof(int)==sizeof(coefficients)/sizeof(float));
 643     float *freq_translated=(float *) malloc(sizeof(float)*(ncoefficients));
 644     freq_translated[0]=1;
 645     //Translate the frequencies in their natural sampling rate to the new sampling rate frequencies
 646     for(size_t i=1;i<ncoefficients-1;++i){
 647         freq_translated[i]=((float)freqs[i]*u->fft_size)/ss.rate;
 648         //pa_log("i: %ld: %d , %g",i,freqs[i],freq_translated[i]);
 649         pa_assert_se(freq_translated[i]>=freq_translated[i-1]);
 650     }
 651     freq_translated[ncoefficients-1]=FLT_MAX;
 652     //Interpolate the specified frequency band values
 653     u->H[0]=1;
 654     for(size_t i=1,j=0;i<(u->fft_size/2+1);++i){
 655         pa_assert_se(j<ncoefficients);
 656         //max frequency range passed, consider the rest as one band
 657         if(freq_translated[j+1]>=FLT_MAX){
 658             for(;i<(u->fft_size/2+1);++i){
 659                 u->H[i]=coefficients[j];
 660             }
 661             break;
 662         }
 663         //pa_log("i: %d, j: %d, freq: %f",i,j,freq_translated[j]);
 664         //pa_log("interp: %0.4f %0.4f",freq_translated[j],freq_translated[j+1]);
 665         pa_assert_se(freq_translated[j]<freq_translated[j+1]);
 666         pa_assert_se(i>=freq_translated[j]);
 667         pa_assert_se(i<=freq_translated[j+1]);
 668         //bilinear-inerpolation of coefficients specified
 669         float c0=(i-freq_translated[j])/(freq_translated[j+1]-freq_translated[j]);
 670         pa_assert_se(c0>=0&&c0<=1.0);
 671         u->H[i]=((1.0f-c0)*coefficients[j]+c0*coefficients[j+1]);
 672         pa_assert_se(u->H[i]>0);
 673         while(i>=floor(freq_translated[j+1])){
 674             j++;
 675         }
 676     }
 677     //divide out the fft gain
 678     for(size_t i=0;i<(u->fft_size/2+1);++i){
 679         u->H[i]/=u->fft_size;
 680     }
 681     free(freq_translated);
 682
 683     /* Create sink */
 684     pa_sink_new_data_init(&sink_data);
 685     sink_data.driver = __FILE__;
 686     sink_data.module = m;
 687     if (!(sink_data.name = pa_xstrdup(pa_modargs_get_value(ma, "sink_name", NULL))))
 688         sink_data.name = pa_sprintf_malloc("%s.equalizer", master->name);
 689     sink_data.namereg_fail = FALSE;
 690     pa_sink_new_data_set_sample_spec(&sink_data, &ss);
 691     pa_sink_new_data_set_channel_map(&sink_data, &map);
 692     z = pa_proplist_gets(master->proplist, PA_PROP_DEVICE_DESCRIPTION);
 693     pa_proplist_sets(sink_data.proplist, PA_PROP_DEVICE_DESCRIPTION, "FFT based equalizer");
 694     pa_proplist_sets(sink_data.proplist, PA_PROP_DEVICE_MASTER_DEVICE, master->name);
 695     pa_proplist_sets(sink_data.proplist, PA_PROP_DEVICE_CLASS, "filter");
 696
 697     if (pa_modargs_get_proplist(ma, "sink_properties", sink_data.proplist, PA_UPDATE_REPLACE) < 0) {
 698         pa_log("Invalid properties");
 699         pa_sink_new_data_done(&sink_data);
 700         goto fail;
 701     }
 702
 703     u->sink = pa_sink_new(m->core, &sink_data, PA_SINK_LATENCY|PA_SINK_DYNAMIC_LATENCY);
 704     pa_sink_new_data_done(&sink_data);
 705
 706     if (!u->sink) {
 707         pa_log("Failed to create sink.");
 708         goto fail;
 709     }
 710
 711     u->sink->parent.process_msg = sink_process_msg;
 712     u->sink->set_state = sink_set_state;
 713     u->sink->update_requested_latency = sink_update_requested_latency;
 714     u->sink->request_rewind = sink_request_rewind;
 715     u->sink->userdata = u;
 716
 717     pa_sink_set_asyncmsgq(u->sink, master->asyncmsgq);
 718     pa_sink_set_rtpoll(u->sink, master->rtpoll);
 719
 720     /* Create sink input */
 721     pa_sink_input_new_data_init(&sink_input_data);
 722     sink_input_data.driver = __FILE__;
 723     sink_input_data.module = m;
 724     sink_input_data.sink = u->master;
 725     pa_proplist_sets(sink_input_data.proplist, PA_PROP_MEDIA_NAME, "Equalized Stream");
 726     pa_proplist_sets(sink_input_data.proplist, PA_PROP_MEDIA_ROLE, "filter");
 727     pa_sink_input_new_data_set_sample_spec(&sink_input_data, &ss);
 728     pa_sink_input_new_data_set_channel_map(&sink_input_data, &map);
 729
 730     pa_sink_input_new(&u->sink_input, m->core, &sink_input_data, PA_SINK_INPUT_DONT_MOVE);
 731     pa_sink_input_new_data_done(&sink_input_data);
 732
 733     if (!u->sink_input)
 734         goto fail;
 735
 736     u->sink_input->pop = sink_input_pop_cb;
 737     u->sink_input->process_rewind = sink_input_process_rewind_cb;
 738     u->sink_input->update_max_rewind = sink_input_update_max_rewind_cb;
 739     u->sink_input->update_max_request = sink_input_update_max_request_cb;
 740     u->sink_input->update_sink_latency_range = sink_input_update_sink_latency_range_cb;
 741     u->sink_input->kill = sink_input_kill_cb;
 742     u->sink_input->attach = sink_input_attach_cb;
 743     u->sink_input->detach = sink_input_detach_cb;
 744     u->sink_input->state_change = sink_input_state_change_cb;
 745     u->sink_input->may_move_to = sink_input_may_move_to_cb;
 746     u->sink_input->userdata = u;
 747
 748     pa_sink_put(u->sink);
 749     pa_sink_input_put(u->sink_input);
 750
 751     pa_modargs_free(ma);
 752
 753     pa_xfree(use_default);
 754
 755     return 0;
 756
 757 fail:
 758     if (ma)
 759         pa_modargs_free(ma);
 760
 761     pa_xfree(use_default);
 762
 763     pa__done(m);
 764
 765     return -1;
 766 }
 767
 768 int pa__get_n_used(pa_module *m) {
 769     struct userdata *u;
 770
 771     pa_assert(m);
 772     pa_assert_se(u = m->userdata);
 773
 774     return pa_sink_linked_by(u->sink);
 775 }
 776
 777 void pa__done(pa_module*m) {
 778     struct userdata *u;
 779
 780     pa_assert(m);
 781
 782     if (!(u = m->userdata))
 783         return;
 784
 785     if (u->sink) {
 786         pa_sink_unlink(u->sink);
 787         pa_sink_unref(u->sink);
 788     }
 789
 790     if (u->sink_input) {
 791         pa_sink_input_unlink(u->sink_input);
 792         pa_sink_input_unref(u->sink_input);
 793     }
 794
 795     if(u->conv_buffer.memblock)
 796         pa_memblock_unref(u->conv_buffer.memblock);
 797
 798     if (u->rendered_q)
 799         pa_memblockq_free(u->rendered_q);
 800
 801     fftwf_destroy_plan(u->inverse_plan);
 802     fftwf_destroy_plan(u->forward_plan);
 803     fftwf_free(u->output_window);
 804     for(size_t c=0;c<u->channels;++c){
 805         fftwf_free(u->output_buffer[c]);
 806         fftwf_free(u->overlap_accum[c]);
 807         fftwf_free(u->input[c]);
 808     }
 809     free(u->output_buffer);
 810     free(u->overlap_accum);
 811     free(u->input);
 812     fftwf_free(u->work_buffer);
 813     fftwf_free(u->W);
 814     fftwf_free(u->H);
 815
 816     pa_xfree(u);
 817 }