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