00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include <glib.h>
00015 #include <math.h>
00016 #include <pthread.h>
00017 #include <string.h>
00018
00019 #include <libaudcore/audstrings.h>
00020 #include <libaudcore/hook.h>
00021
00022 #include "equalizer.h"
00023 #include "misc.h"
00024 #include "types.h"
00025
00026 #define EQ_BANDS AUD_EQUALIZER_NBANDS
00027 #define MAX_CHANNELS 10
00028
00029
00030
00031 #define Q 1.2247449
00032
00033
00034
00035
00036
00037 static const float CF[EQ_BANDS] = {31.25, 62.5, 125, 250, 500, 1000, 2000,
00038 4000, 8000, 16000};
00039
00040 static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
00041 static bool_t active;
00042 static int channels, rate;
00043 static float a[EQ_BANDS][2];
00044 static float b[EQ_BANDS][2];
00045 static float wqv[MAX_CHANNELS][EQ_BANDS][2];
00046 static float gv[MAX_CHANNELS][EQ_BANDS];
00047 static int K;
00048
00049
00050 static void bp2 (float * a, float * b, float fc, float q)
00051 {
00052 float th = 2 * M_PI * fc;
00053 float C = (1 - tanf (th * q / 2)) / (1 + tanf (th * q / 2));
00054
00055 a[0] = (1 + C) * cosf (th);
00056 a[1] = -C;
00057 b[0] = (1 - C) / 2;
00058 b[1] = -1.005;
00059 }
00060
00061 void eq_set_format (int new_channels, int new_rate)
00062 {
00063 int k;
00064
00065 pthread_mutex_lock (& mutex);
00066
00067 channels = new_channels;
00068 rate = new_rate;
00069
00070
00071 K = EQ_BANDS;
00072
00073 while (CF[K - 1] > (float) rate / 2.2)
00074 K --;
00075
00076
00077 for (k = 0; k < K; k ++)
00078 bp2 (a[k], b[k], CF[k] / (float) rate, Q);
00079
00080
00081 memset (wqv[0][0], 0, sizeof wqv);
00082
00083 pthread_mutex_unlock (& mutex);
00084 }
00085
00086 static void eq_set_bands_real (double preamp, double * values)
00087 {
00088 float adj[EQ_BANDS];
00089 for (int i = 0; i < EQ_BANDS; i ++)
00090 adj[i] = preamp + values[i];
00091
00092 for (int c = 0; c < MAX_CHANNELS; c ++)
00093 for (int i = 0; i < EQ_BANDS; i ++)
00094 gv[c][i] = pow (10, adj[i] / 20) - 1;
00095 }
00096
00097 void eq_filter (float * data, int samples)
00098 {
00099 int channel;
00100
00101 pthread_mutex_lock (& mutex);
00102
00103 if (! active)
00104 {
00105 pthread_mutex_unlock (& mutex);
00106 return;
00107 }
00108
00109 for (channel = 0; channel < channels; channel ++)
00110 {
00111 float * g = gv[channel];
00112 float * end = data + samples;
00113 float * f;
00114
00115 for (f = data + channel; f < end; f += channels)
00116 {
00117 int k;
00118 float yt = * f;
00119
00120 for (k = 0; k < K; k ++)
00121 {
00122
00123 float * wq = wqv[channel][k];
00124
00125 float w = yt * b[k][0] + wq[0] * a[k][0] + wq[1] * a[k][1];
00126
00127
00128 yt += (w + wq[1] * b[k][1]) * g[k];
00129
00130
00131 wq[1] = wq[0];
00132 wq[0] = w;
00133 }
00134
00135
00136 * f = yt;
00137 }
00138 }
00139
00140 pthread_mutex_unlock (& mutex);
00141 }
00142
00143 static void eq_update (void * data, void * user)
00144 {
00145 pthread_mutex_lock (& mutex);
00146
00147 active = get_bool (NULL, "equalizer_active");
00148
00149 double values[EQ_BANDS];
00150 eq_get_bands (values);
00151 eq_set_bands_real (get_double (NULL, "equalizer_preamp"), values);
00152
00153 pthread_mutex_unlock (& mutex);
00154 }
00155
00156 void eq_init (void)
00157 {
00158 eq_update (NULL, NULL);
00159 hook_associate ("set equalizer_active", eq_update, NULL);
00160 hook_associate ("set equalizer_preamp", eq_update, NULL);
00161 hook_associate ("set equalizer_bands", eq_update, NULL);
00162 }
00163
00164 void eq_cleanup (void)
00165 {
00166 hook_dissociate ("set equalizer_active", eq_update);
00167 hook_dissociate ("set equalizer_preamp", eq_update);
00168 hook_dissociate ("set equalizer_bands", eq_update);
00169 }
00170
00171 void eq_set_bands (const double * values)
00172 {
00173 char * string = double_array_to_string (values, EQ_BANDS);
00174 g_return_if_fail (string);
00175 set_string (NULL, "equalizer_bands", string);
00176 g_free (string);
00177 }
00178
00179 void eq_get_bands (double * values)
00180 {
00181 memset (values, 0, sizeof (double) * EQ_BANDS);
00182 char * string = get_string (NULL, "equalizer_bands");
00183 string_to_double_array (string, values, EQ_BANDS);
00184 g_free (string);
00185 }
00186
00187 void eq_set_band (int band, double value)
00188 {
00189 g_return_if_fail (band >= 0 && band < EQ_BANDS);
00190 double values[EQ_BANDS];
00191 eq_get_bands (values);
00192 values[band] = value;
00193 eq_set_bands (values);
00194 }
00195
00196 double eq_get_band (int band)
00197 {
00198 g_return_val_if_fail (band >= 0 && band < EQ_BANDS, 0);
00199 double values[EQ_BANDS];
00200 eq_get_bands (values);
00201 return values[band];
00202 }