Comment out the correct #endif directive.
[mplayer/greg.git] / libaf / af_surround.c
blob5966e725fbf8ecc8a7c882b1fe158cc0ba51837e
1 /*
2 This is an libaf filter to do simple decoding of matrixed surround
3 sound. This will provide a (basic) surround-sound effect from
4 audio encoded for Dolby Surround, Pro Logic etc.
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
16 * You should have received a copy of the GNU General Public License along
17 * with this program; if not, write to the Free Software Foundation, Inc.,
18 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20 Original author: Steve Davies <steve@daviesfam.org>
23 /* The principle: Make rear channels by extracting anti-phase data
24 from the front channels, delay by 20ms and feed to rear in anti-phase
28 /* SPLITREAR: Define to decode two distinct rear channels - this
29 doesn't work so well in practice because separation in a passive
30 matrix is not high. C (dialogue) to Ls and Rs 14dB or so - so
31 dialogue leaks to the rear. Still - give it a try and send
32 feedback. Comment this define for old behavior of a single
33 surround sent to rear in anti-phase */
34 #define SPLITREAR 1
36 #include <stdio.h>
37 #include <stdlib.h>
38 #include <string.h>
40 #include "af.h"
41 #include "dsp.h"
43 #define L 32 // Length of fir filter
44 #define LD 65536 // Length of delay buffer
46 // 32 Tap fir filter loop unrolled
47 #define FIR(x,w,y) \
48 y = ( w[0] *x[0] +w[1] *x[1] +w[2] *x[2] +w[3] *x[3] \
49 + w[4] *x[4] +w[5] *x[5] +w[6] *x[6] +w[7] *x[7] \
50 + w[8] *x[8] +w[9] *x[9] +w[10]*x[10]+w[11]*x[11] \
51 + w[12]*x[12]+w[13]*x[13]+w[14]*x[14]+w[15]*x[15] \
52 + w[16]*x[16]+w[17]*x[17]+w[18]*x[18]+w[19]*x[19] \
53 + w[20]*x[20]+w[21]*x[21]+w[22]*x[22]+w[23]*x[23] \
54 + w[24]*x[24]+w[25]*x[25]+w[26]*x[26]+w[27]*x[27] \
55 + w[28]*x[28]+w[29]*x[29]+w[30]*x[30]+w[31]*x[31])
57 // Add to circular queue macro + update index
58 #ifdef SPLITREAR
59 #define ADDQUE(qi,rq,lq,r,l)\
60 lq[qi]=lq[qi+L]=(l);\
61 rq[qi]=rq[qi+L]=(r);\
62 qi=(qi-1)&(L-1);
63 #else
64 #define ADDQUE(qi,lq,l)\
65 lq[qi]=lq[qi+L]=(l);\
66 qi=(qi-1)&(L-1);
67 #endif
69 // Macro for updating queue index in delay queues
70 #define UPDATEQI(qi) qi=(qi+1)&(LD-1)
72 // instance data
73 typedef struct af_surround_s
75 float lq[2*L]; // Circular queue for filtering left rear channel
76 float rq[2*L]; // Circular queue for filtering right rear channel
77 float w[L]; // FIR filter coefficients for surround sound 7kHz low-pass
78 float* dr; // Delay queue right rear channel
79 float* dl; // Delay queue left rear channel
80 float d; // Delay time
81 int i; // Position in circular buffer
82 int wi; // Write index for delay queue
83 int ri; // Read index for delay queue
84 }af_surround_t;
86 // Initialization and runtime control
87 static int control(struct af_instance_s* af, int cmd, void* arg)
89 af_surround_t *s = af->setup;
90 switch(cmd){
91 case AF_CONTROL_REINIT:{
92 float fc;
93 af->data->rate = ((af_data_t*)arg)->rate;
94 af->data->nch = ((af_data_t*)arg)->nch*2;
95 af->data->format = AF_FORMAT_FLOAT_NE;
96 af->data->bps = 4;
98 if (af->data->nch != 4){
99 af_msg(AF_MSG_ERROR,"[surround] Only stereo input is supported.\n");
100 return AF_DETACH;
102 // Surround filer coefficients
103 fc = 2.0 * 7000.0/(float)af->data->rate;
104 if (-1 == af_filter_design_fir(L, s->w, &fc, LP|HAMMING, 0)){
105 af_msg(AF_MSG_ERROR,"[surround] Unable to design low-pass filter.\n");
106 return AF_ERROR;
109 // Free previous delay queues
110 if(s->dl)
111 free(s->dl);
112 if(s->dr)
113 free(s->dr);
114 // Allocate new delay queues
115 s->dl = calloc(LD,af->data->bps);
116 s->dr = calloc(LD,af->data->bps);
117 if((NULL == s->dl) || (NULL == s->dr))
118 af_msg(AF_MSG_FATAL,"[delay] Out of memory\n");
120 // Initialize delay queue index
121 if(AF_OK != af_from_ms(1, &s->d, &s->wi, af->data->rate, 0.0, 1000.0))
122 return AF_ERROR;
123 // printf("%i\n",s->wi);
124 s->ri = 0;
126 if((af->data->format != ((af_data_t*)arg)->format) ||
127 (af->data->bps != ((af_data_t*)arg)->bps)){
128 ((af_data_t*)arg)->format = af->data->format;
129 ((af_data_t*)arg)->bps = af->data->bps;
130 return AF_FALSE;
132 return AF_OK;
134 case AF_CONTROL_COMMAND_LINE:{
135 float d = 0;
136 sscanf((char*)arg,"%f",&d);
137 if ((d < 0) || (d > 1000)){
138 af_msg(AF_MSG_ERROR,"[surround] Invalid delay time, valid time values"
139 " are 0ms to 1000ms current value is %0.3f ms\n",d);
140 return AF_ERROR;
142 s->d = d;
143 return AF_OK;
146 return AF_UNKNOWN;
149 // Deallocate memory
150 static void uninit(struct af_instance_s* af)
152 if(af->data)
153 free(af->data->audio);
154 free(af->data);
155 free(af->setup);
158 // The beginnings of an active matrix...
159 static float steering_matrix[][12] = {
160 // LL RL LR RR LS RS
161 // LLs RLs LRs RRs LC RC
162 {.707, .0, .0, .707, .5, -.5,
163 .5878, -.3928, .3928, -.5878, .5, .5},
166 // Experimental moving average dominance
167 //static int amp_L = 0, amp_R = 0, amp_C = 0, amp_S = 0;
169 // Filter data through filter
170 static af_data_t* play(struct af_instance_s* af, af_data_t* data){
171 af_surround_t* s = (af_surround_t*)af->setup;
172 float* m = steering_matrix[0];
173 float* in = data->audio; // Input audio data
174 float* out = NULL; // Output audio data
175 float* end = in + data->len / sizeof(float); // Loop end
176 int i = s->i; // Filter queue index
177 int ri = s->ri; // Read index for delay queue
178 int wi = s->wi; // Write index for delay queue
180 if (AF_OK != RESIZE_LOCAL_BUFFER(af, data))
181 return NULL;
183 out = af->data->audio;
185 while(in < end){
186 /* Dominance:
187 abs(in[0]) abs(in[1]);
188 abs(in[0]+in[1]) abs(in[0]-in[1]);
189 10 * log( abs(in[0]) / (abs(in[1])|1) );
190 10 * log( abs(in[0]+in[1]) / (abs(in[0]-in[1])|1) ); */
192 /* About volume balancing...
193 Surround encoding does the following:
194 Lt=L+.707*C+.707*S, Rt=R+.707*C-.707*S
195 So S should be extracted as:
196 (Lt-Rt)
197 But we are splitting the S to two output channels, so we
198 must take 3dB off as we split it:
199 Ls=Rs=.707*(Lt-Rt)
200 Trouble is, Lt could be +1, Rt -1, so possibility that S will
201 overflow. So to avoid that, we cut L/R by 3dB (*.707), and S by
202 6dB (/2). This keeps the overall balance, but guarantees no
203 overflow. */
205 // Output front left and right
206 out[0] = m[0]*in[0] + m[1]*in[1];
207 out[1] = m[2]*in[0] + m[3]*in[1];
209 // Low-pass output @ 7kHz
210 FIR((&s->lq[i]), s->w, s->dl[wi]);
212 // Delay output by d ms
213 out[2] = s->dl[ri];
215 #ifdef SPLITREAR
216 // Low-pass output @ 7kHz
217 FIR((&s->rq[i]), s->w, s->dr[wi]);
219 // Delay output by d ms
220 out[3] = s->dr[ri];
221 #else
222 out[3] = -out[2];
223 #endif
225 // Update delay queues indexes
226 UPDATEQI(ri);
227 UPDATEQI(wi);
229 // Calculate and save surround in circular queue
230 #ifdef SPLITREAR
231 ADDQUE(i, s->rq, s->lq, m[6]*in[0]+m[7]*in[1], m[8]*in[0]+m[9]*in[1]);
232 #else
233 ADDQUE(i, s->lq, m[4]*in[0]+m[5]*in[1]);
234 #endif
236 // Next sample...
237 in = &in[data->nch];
238 out = &out[af->data->nch];
241 // Save indexes
242 s->i = i; s->ri = ri; s->wi = wi;
244 // Set output data
245 data->audio = af->data->audio;
246 data->len *= 2;
247 data->nch = af->data->nch;
249 return data;
252 static int af_open(af_instance_t* af){
253 af->control=control;
254 af->uninit=uninit;
255 af->play=play;
256 af->mul=2;
257 af->data=calloc(1,sizeof(af_data_t));
258 af->setup=calloc(1,sizeof(af_surround_t));
259 if(af->data == NULL || af->setup == NULL)
260 return AF_ERROR;
261 ((af_surround_t*)af->setup)->d = 20;
262 return AF_OK;
265 af_info_t af_info_surround =
267 "Surround decoder filter",
268 "surround",
269 "Steve Davies <steve@daviesfam.org>",
271 AF_FLAGS_NOT_REENTRANT,
272 af_open