← Back to branch summary

~ubuntu-branches/ubuntu/wily/aliki/wily

« back to all changes in this revision

Viewing changes to source/convolve.cc

  • Committer: Bazaar Package Importer
  • Author(s): Jaromír Mikeš
  • Date: 2011-07-26 21:12:17 UTC
  • Revision ID: james.westby@ubuntu.com-20110726211217-v948cgyyjokiq3sl
Tags: upstream-0.1.0
Import upstream version 0.1.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
source/convolve.cc
 
1
// -------------------------------------------------------------------------
 
2
//
 
3
//    Copyright (C) 2005-2007 Fons Adriaensen <fons@kokkinizita.net>
 
4
//    
 
5
//    This program is free software; you can redistribute it and/or modify
 
6
//    it under the terms of the GNU General Public License as published by
 
7
//    the Free Software Foundation; either version 2 of the License, or
 
8
//    (at your option) any later version.
 
9
//
 
10
//    This program is distributed in the hope that it will be useful,
 
11
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
 
12
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
13
//    GNU General Public License for more details.
 
14
//
 
15
//    You should have received a copy of the GNU General Public License
 
16
//    along with this program; if not, write to the Free Software
 
17
//    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 
18
//
 
19
// -------------------------------------------------------------------------
 
20
 
 
21
 
 
22
#define _XOPEN_SOURCE 600 // for posix_memalign()
 
23
 
 
24
 
 
25
#include <stdlib.h>
 
26
#include <stdio.h>
 
27
#include <string.h>
 
28
#include "convolve.h"
 
29
 
 
30
 
 
31
#ifdef USE_SSE_AND_3DN
 
32
extern "C" void mac_sse (fftwf_complex *A, fftwf_complex *B, fftwf_complex *S, int N);
 
33
extern "C" void mac_3dn (fftwf_complex *A, fftwf_complex *B, fftwf_complex *S, int N);
 
34
#else
 
35
extern "C" void mac_sse (fftwf_complex *A, fftwf_complex *B, fftwf_complex *S, int N) {}
 
36
extern "C" void mac_3dn (fftwf_complex *A, fftwf_complex *B, fftwf_complex *S, int N) {}
 
37
#endif
 
38
 
 
39
 
 
40
Convdata::Convdata (size_t part, size_t size, unsigned int opts) :
 
41
    _refc (0), _part (part), _opts (opts), _nact (0), _buff (0), _plan (0)
 
42
{
 
43
    unsigned int i;
 
44
 
 
45
    if (   (part < 0x000040)
 
46
        || (part > 0x010000)
 
47
        || (part & (part - 1))
 
48
        || (size > 0x400000)
 
49
        || (size > 0x1000 * part)) abort ();
 
50
 
 
51
#ifndef USE_SSE_AND_3DN
 
52
    _opts = 0;
 
53
#endif
 
54
 
 
55
    _npar = (size + part - 1) / part; 
 
56
    _norm = 0.5f / part;
 
57
    _fftd = new fftwf_complex * [_npar]; 
 
58
    for (i = 0; i < _npar; i++) _fftd [i] = 0;
 
59
}
 
60
 
 
61
 
 
62
Convdata::~Convdata (void)
 
63
{
 
64
    unsigned int i;
 
65
 
 
66
    prepare_done ();
 
67
    for (i = 0; i < _npar; i++) free (_fftd [i]);
 
68
    delete[] _fftd;
 
69
}
 
70
 
 
71
 
 
72
void Convdata::prepare_part (unsigned int ipar, float gain, float *data, int step)
 
73
{
 
74
    unsigned int i;
 
75
 
 
76
    if (ipar >= _npar) abort ();
 
77
 
 
78
    if (data)
 
79
    {
 
80
        gain *= _norm;
 
81
        if (! _plan)
 
82
        { 
 
83
            posix_memalign ((void **)(&_buff), 16, 2 * _part * sizeof (float));
 
84
            memset (_buff + _part, 0, _part * sizeof (float));  
 
85
            _plan = fftwf_plan_dft_r2c_1d (2 * _part, _buff, _fftd [ipar], FFTW_ESTIMATE);
 
86
        }
 
87
        for (i = 0; i < _part; i++) _buff [i] = data [i * step] * gain;
 
88
        if (! _fftd [ipar]);
 
89
        {
 
90
            posix_memalign ((void **)(_fftd + ipar), 16, (_part + 1) * sizeof (fftwf_complex));
 
91
            _nact++;
 
92
        }
 
93
        fftwf_execute_dft_r2c (_plan, _buff, _fftd [ipar]);
 
94
        if (_opts) swap (_fftd [ipar], _part);
 
95
    }
 
96
    else if (_fftd [ipar])
 
97
    {
 
98
        free (_fftd [ipar]);
 
99
        _fftd [ipar] = 0;
 
100
        _nact--;
 
101
    }
 
102
}
 
103
 
 
104
 
 
105
void Convdata::prepare_done (void)
 
106
{
 
107
    if (_plan)
 
108
    {
 
109
        fftwf_free (_buff);
 
110
        fftwf_destroy_plan (_plan);
 
111
        _buff = 0;
 
112
        _plan = 0;
 
113
    }
 
114
}
 
115
 
 
116
 
 
117
void Convdata::swap (fftwf_complex *p, size_t n)
 
118
{
 
119
    float a, b;
 
120
 
 
121
    while (n)
 
122
    {
 
123
        a = p [2][0];
 
124
        b = p [3][0];
 
125
        p [2][0] = p [0][1];
 
126
        p [3][0] = p [1][1];
 
127
        p [0][1] = a;
 
128
        p [1][1] = b;
 
129
        p += 4;
 
130
        n -= 4;
 
131
    }
 
132
}
 
133
 
 
134
 
 
135
 
 
136
 
 
137
Convolver::Convolver (size_t part, size_t size, unsigned int opts, unsigned int nip, unsigned int nop) :
 
138
    _part (part), _opts (opts), _nip (nip), _nop (nop)
 
139
{
 
140
    unsigned int i;
 
141
 
 
142
    if (   (part < 0x000040)
 
143
        || (part > 0x010000)
 
144
        || (part & (part - 1))
 
145
        || (size > 0x400000)
 
146
        || (size > 4096 * part)
 
147
        || (nip > 256)
 
148
        || (nop > 256)
 
149
        || (nip * nop > 1024)) abort ();
 
150
 
 
151
#ifndef USE_SSE_AND_3DN
 
152
    _opts = 0;
 
153
#endif
 
154
 
 
155
    _npar = (size + part - 1) / part;
 
156
    _conv = new Convdata *[_nip * _nop];
 
157
    for (i = 0; i < _nip * _nop; i++) _conv [i] = 0;
 
158
    posix_memalign ((void **)(&_ip_buff), 16, (_nip + 1) * _part * sizeof (float));
 
159
    posix_memalign ((void **)(&_oA_buff), 16, 2 * _nop * _part * sizeof (float));
 
160
    posix_memalign ((void **)(&_oB_buff), 16, 2 * _nop * _part * sizeof (float));
 
161
    posix_memalign ((void **)(&_mac_data), 16, (_part + 1) * sizeof (fftwf_complex));
 
162
    _fwd_data = new  fftwf_complex * [_nip * _npar]; 
 
163
    for (i = 0; i < _nip * _npar; i++)
 
164
    {
 
165
        posix_memalign ((void **)(_fwd_data + i), 16, (_part + 1) * sizeof (fftwf_complex));
 
166
    }
 
167
    _fwd_plan = fftwf_plan_dft_r2c_1d (2 * part, _ip_buff, _mac_data, FFTW_ESTIMATE);
 
168
    _rev_plan = fftwf_plan_dft_c2r_1d (2 * part, _mac_data, _oA_buff, FFTW_ESTIMATE);
 
169
    reset ();
 
170
}
 
171
 
 
172
 
 
173
 
 
174
Convolver::~Convolver (void)
 
175
{
 
176
    unsigned int i;
 
177
 
 
178
    fftwf_destroy_plan (_fwd_plan);
 
179
    fftwf_destroy_plan (_rev_plan);
 
180
    for (i = 0; i < _nip * _npar; i++) free (_fwd_data [i]);
 
181
    delete[] _fwd_data;
 
182
    free (_ip_buff);
 
183
    free (_oA_buff);
 
184
    free (_oB_buff);
 
185
    free (_mac_data);
 
186
    for (i = 0; i < _nip * _nop; i++)
 
187
    {
 
188
        if (_conv [i] && (_conv [i]->dec_refc () == 0)) delete _conv [i];
 
189
    }
 
190
    delete[] _conv;
 
191
}
 
192
 
 
193
 
 
194
int Convolver::set_conv (unsigned int ip, unsigned int op, Convdata *C)
 
195
{
 
196
    unsigned int i;
 
197
 
 
198
    if ((ip >= _nip) || (op >= _nop)) return -1;
 
199
    i = ip + op * _nip;
 
200
 
 
201
    if (_conv [i] && (_conv [i]->dec_refc () == 0))
 
202
    {
 
203
        delete _conv [i];
 
204
        _conv [i] = 0;
 
205
    }
 
206
    if (C)
 
207
    {
 
208
        if (C->_part != _part) return -1; 
 
209
        C->inc_refc ();
 
210
        _conv [i] = C;
 
211
    }
 
212
    return 0;
 
213
}
 
214
 
 
215
 
 
216
void Convolver::reset ()
 
217
{
 
218
    unsigned int i;
 
219
 
 
220
    memset (_ip_buff, 0, (_nip + 1) * _part * sizeof (float));
 
221
    memset (_oA_buff, 0,   2 * _nop * _part * sizeof (float));
 
222
    memset (_oB_buff, 0,   2 * _nop * _part * sizeof (float));
 
223
    for (i = 0; i < _nip * _npar; i++)
 
224
    {
 
225
        memset (_fwd_data [i], 0, (_part + 1) * sizeof (fftwf_complex));
 
226
    }
 
227
    _iter = 0;
 
228
    _offs = 0; 
 
229
    _op_buff = _oB_buff;
 
230
}
 
231
 
 
232
 
 
233
void Convolver::process ()
 
234
{
 
235
    unsigned int    g, h, i, j;
 
236
    float           *p1, *p2;
 
237
    fftwf_complex   *q1, *q2; 
 
238
    Convdata        *C;
 
239
 
 
240
    if (_offs == 0) _offs = _npar;
 
241
    _offs--;
 
242
 
 
243
    p1 = _ip_buff + _nip * _part;
 
244
    for (i = 0; i < _nip;  i++)
 
245
    {
 
246
        memset (p1, 0, _part * sizeof (float));
 
247
        p1 -= _part;
 
248
        q1 = _fwd_data [_offs + (_nip - 1 - i) * _npar]; 
 
249
        fftwf_execute_dft_r2c (_fwd_plan, p1, q1);
 
250
        if (_opts) Convdata::swap (q1, _part);
 
251
    }
 
252
 
 
253
    if (++_iter & 1)
 
254
    {  
 
255
        p1 = _oA_buff;
 
256
        p2 = _oB_buff + _part;
 
257
    }
 
258
    else 
 
259
    {
 
260
        p1 = _oB_buff;
 
261
        p2 = _oA_buff + _part;
 
262
    }
 
263
    _op_buff = p1;
 
264
    for (j = 0; j < _nop; j++)
 
265
    {
 
266
        memset (_mac_data, 0, (_part + 1) * sizeof (fftwf_complex));
 
267
        for (i = 0; i < _nip; i++)
 
268
        {
 
269
            C = _conv [i + j * _nip];
 
270
            if (C && C->_nact)
 
271
            {
 
272
                for (h = 0; h < _npar; h++)
 
273
                {
 
274
                    q2 = C->_fftd [h];
 
275
                    if (q2)
 
276
                    {
 
277
                        g = h + _offs;
 
278
                        if (g >= _npar) g -= _npar;            
 
279
                        q1 = _fwd_data [g + i * _npar];
 
280
                        if      (_opts == CONV_OPT_SSE) mac_sse (q1, q2, _mac_data, _part >> 2); 
 
281
                        else if (_opts == CONV_OPT_3DN) mac_3dn (q1, q2, _mac_data, _part >> 2);
 
282
                        else for (g = 0; g <= _part; g++)
 
283
                        {
 
284
                            _mac_data [g][0] += q1 [g][0] * q2 [g][0] - q1 [g][1] * q2 [g][1];
 
285
                            _mac_data [g][1] += q1 [g][0] * q2 [g][1] + q1 [g][1] * q2 [g][0];
 
286
                        }
 
287
                    }
 
288
                }  
 
289
            }
 
290
        }
 
291
 
 
292
        if (_opts) Convdata::swap (_mac_data, _part);
 
293
        fftwf_execute_dft_c2r (_rev_plan, _mac_data, p1);
 
294
        for (g = 0; g < _part; g++) *p1++ += *p2++;
 
295
        p1 += _part;
 
296
        p2 += _part;
 
297
    }
 
298
}