iodelay.cpp 7.12 KB
Newer Older
sletz's avatar
sletz committed
1
/*
sletz's avatar
sletz committed
2
    Copyright (C) 2003-2008 Fons Adriaensen <fons@kokkinizita.net>
3
    
sletz's avatar
sletz committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

// --------------------------------------------------------------------------------

#include <stdlib.h>
#include <stdio.h>
23
#define __STDC_LIMIT_MACROS
24
#include <stdint.h>
25
#include <math.h>
sletz's avatar
sletz committed
26
#include <unistd.h>
sletz's avatar
sletz committed
27
#include <jack/jack.h>
sletz's avatar
sletz committed
28

29
struct Freq
sletz's avatar
sletz committed
30
31
32
33
34
{
    int   p;
    int   f;
    float xa;
    float ya;
35
36
37
38
    float x1;
    float y1;
    float x2;
    float y2;
sletz's avatar
sletz committed
39
40
41
};


42
43
struct MTDM
{
sletz's avatar
sletz committed
44
45
    double  _del;
    double  _err;
46
    float   _wlp;
sletz's avatar
sletz committed
47
48
    int     _cnt;
    int     _inv;
49
50

    struct Freq _freq [13];
sletz's avatar
sletz committed
51
52
};

53
54

struct MTDM * mtdm_new (double fsamp)
sletz's avatar
sletz committed
55
56
{
    int   i;
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
    struct Freq  *F;

    struct MTDM *retval = (MTDM *)malloc( sizeof(struct MTDM) );

    if (retval==NULL)
	    return NULL;

    retval->_cnt = 0;
    retval->_inv = 0;

    retval->_freq [0].f  = 4096;
    retval->_freq [1].f  = 2048;
    retval->_freq [2].f  = 3072;
    retval->_freq [3].f  = 2560;
    retval->_freq [4].f  = 2304;
    retval->_freq [5].f  = 2176; 
    retval->_freq [6].f  = 1088;
    retval->_freq [7].f  = 1312;
    retval->_freq [8].f  = 1552;
    retval->_freq [9].f  = 1800;
    retval->_freq [10].f = 3332;
    retval->_freq [11].f = 3586;
    retval->_freq [12].f = 3841;
    retval->_wlp = 200.0f / fsamp;
    for (i = 0, F = retval->_freq; i < 13; i++, F++) {
Stephane Letz's avatar
Stephane Letz committed
82
83
        F->p = 128;
        F->xa = F->ya = 0.0f;
84
85
        F->x1 = F->y1 = 0.0f;
        F->x2 = F->y2 = 0.0f;
sletz's avatar
sletz committed
86
    }
87
88

    return retval;
sletz's avatar
sletz committed
89
90
}

91
int mtdm_process (struct MTDM *self, size_t len, float *ip, float *op)
sletz's avatar
sletz committed
92
93
94
{
    int    i;
    float  vip, vop, a, c, s;
95
    struct Freq   *F;
sletz's avatar
sletz committed
96
97
98
99

    while (len--)
    {
        vop = 0.0f;
Stephane Letz's avatar
Stephane Letz committed
100
        vip = *ip++;
101
        for (i = 0, F = self->_freq; i < 13; i++, F++)
Stephane Letz's avatar
Stephane Letz committed
102
        {
103
            a = 2 * (float) M_PI * (F->p & 65535) / 65536.0; 
Stephane Letz's avatar
Stephane Letz committed
104
            F->p += F->f;
105
106
107
            c =  cosf (a); 
            s = -sinf (a); 
            vop += (i ? 0.01f : 0.20f) * s;
Stephane Letz's avatar
Stephane Letz committed
108
109
            F->xa += s * vip;
            F->ya += c * vip;
110
        } 
Stephane Letz's avatar
Stephane Letz committed
111
        *op++ = vop;
112
        if (++self->_cnt == 16)
Stephane Letz's avatar
Stephane Letz committed
113
        {
114
            for (i = 0, F = self->_freq; i < 13; i++, F++)
Stephane Letz's avatar
Stephane Letz committed
115
            {
116
117
118
119
                F->x1 += self->_wlp * (F->xa - F->x1 + 1e-20);
                F->y1 += self->_wlp * (F->ya - F->y1 + 1e-20);
                F->x2 += self->_wlp * (F->x1 - F->x2 + 1e-20);
                F->y2 += self->_wlp * (F->y1 - F->y2 + 1e-20);
Stephane Letz's avatar
Stephane Letz committed
120
121
                F->xa = F->ya = 0.0f;
            }
122
            self->_cnt = 0;
Stephane Letz's avatar
Stephane Letz committed
123
        }
sletz's avatar
sletz committed
124
125
126
127
128
    }

    return 0;
}

129
int mtdm_resolve (struct MTDM *self)
sletz's avatar
sletz committed
130
131
132
{
    int     i, k, m;
    double  d, e, f0, p;
133
    struct Freq *F = self->_freq;
sletz's avatar
sletz committed
134

135
136
137
138
139
    if (hypot (F->x2, F->y2) < 0.001) return -1;
    d = atan2 (F->y2, F->x2) / (2 * M_PI);
    if (self->_inv) d += 0.5;
    if (d > 0.5) d -= 1.0;
    f0 = self->_freq [0].f;
sletz's avatar
sletz committed
140
    m = 1;
141
142
    self->_err = 0.0;
    for (i = 0; i < 12; i++)
sletz's avatar
sletz committed
143
    {
Stephane Letz's avatar
Stephane Letz committed
144
        F++;
145
146
        p = atan2 (F->y2, F->x2) / (2 * M_PI) - d * F->f / f0;
        if (self->_inv) p += 0.5;
Stephane Letz's avatar
Stephane Letz committed
147
        p -= floor (p);
148
        p *= 2;
Stephane Letz's avatar
Stephane Letz committed
149
150
        k = (int)(floor (p + 0.5));
        e = fabs (p - k);
151
152
153
154
155
156
        if (e > self->_err) self->_err = e;
        if (e > 0.4) return 1; 
        d += m * (k & 1);
        m *= 2;
    }  
    self->_del = 16 * d;
sletz's avatar
sletz committed
157
158
159
160

    return 0;
}

161
162
163
164
void mtdm_invert (struct MTDM *self)
{
	self->_inv ^= 1;
}
sletz's avatar
sletz committed
165
// --------------------------------------------------------------------------------
sletz's avatar
sletz committed
166

167
static struct MTDM    *mtdm;
sletz's avatar
sletz committed
168
169
170
171
static jack_client_t  *jack_handle;
static jack_port_t    *jack_capt;
static jack_port_t    *jack_play;

172
173
jack_latency_range_t   capture_latency = {UINT32_MAX, UINT32_MAX};
jack_latency_range_t   playback_latency = {UINT32_MAX, UINT32_MAX};
sletz's avatar
sletz committed
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199

void
latency_cb (jack_latency_callback_mode_t mode, void *arg)
{
	jack_latency_range_t range;

	range.min = range.max = 0;

	if (mode == JackCaptureLatency) {
		jack_port_set_latency_range (jack_play, mode, &range);
		jack_port_get_latency_range (jack_capt, mode, &range);
		if ((range.min != capture_latency.min) || (range.max != capture_latency.max)) {
			capture_latency = range;
			printf ("new capture latency: [%d, %d]\n", range.min, range.max);
		}
	} else {
		jack_port_set_latency_range (jack_capt, mode, &range);
		jack_port_get_latency_range (jack_play, mode, &range);
		if ((range.min != playback_latency.min) || (range.max != playback_latency.max)) {
			playback_latency = range;
			printf ("new playback latency: [%d, %d]\n", range.min, range.max);
		}
	}

}

sletz's avatar
sletz committed
200
int jack_callback (jack_nframes_t nframes, void *arg)
sletz's avatar
sletz committed
201
202
203
204
205
{
    float *ip, *op;

    ip = (float *)(jack_port_get_buffer (jack_capt, nframes));
    op = (float *)(jack_port_get_buffer (jack_play, nframes));
206
    mtdm_process (mtdm, nframes, ip, op);
sletz's avatar
sletz committed
207
    return 0;
sletz's avatar
sletz committed
208
209
210
211
}

int main (int ac, char *av [])
{
sletz's avatar
sletz committed
212
213
    float          t;
    jack_status_t  s;
sletz's avatar
sletz committed
214

sletz's avatar
sletz committed
215
216
    jack_handle = jack_client_open ("jack_delay", JackNoStartServer, &s);
    if (jack_handle == 0)
sletz's avatar
sletz committed
217
    {
sletz's avatar
sletz committed
218
219
        fprintf (stderr, "Can't connect to Jack, is the server running ?\n");
        exit (1);
sletz's avatar
sletz committed
220
221
    }

222
223
    mtdm = mtdm_new(jack_get_sample_rate(jack_handle));

sletz's avatar
sletz committed
224
    jack_set_process_callback (jack_handle, jack_callback, 0);
sletz's avatar
sletz committed
225

sletz's avatar
sletz committed
226
227
228
    if (jack_set_latency_callback)
	    jack_set_latency_callback (jack_handle, latency_cb, 0);

sletz's avatar
sletz committed
229
230
231
    jack_capt = jack_port_register (jack_handle, "in",  JACK_DEFAULT_AUDIO_TYPE, JackPortIsInput, 0);
    jack_play = jack_port_register (jack_handle, "out", JACK_DEFAULT_AUDIO_TYPE, JackPortIsOutput, 0);

sletz's avatar
sletz committed
232
233
    t = 1000.0f / jack_get_sample_rate (jack_handle);

sletz's avatar
sletz committed
234
235
    if (jack_activate (jack_handle))
    {
sletz's avatar
sletz committed
236
        fprintf(stderr, "Can't activate Jack");
sletz's avatar
sletz committed
237
238
239
240
241
        return 1;
    }

    while (1)
    {
242
243
244
245
246
247
248
249
 
    #ifdef WIN32 
        Sleep (250); 
    #else 
        usleep (250000); 
 	#endif        
        if (mtdm_resolve (mtdm) < 0) printf ("Signal below threshold...\n");
        else 
sletz's avatar
sletz committed
250
        {
251
            jack_nframes_t systemic_latency;
252
253

            if (mtdm->_err > 0.3) 
sletz's avatar
sletz committed
254
            {
255
256
                mtdm_invert ( mtdm );
                mtdm_resolve ( mtdm );
sletz's avatar
sletz committed
257
            }
258
            systemic_latency = (jack_nframes_t) floor (mtdm->_del - (capture_latency.max + playback_latency.max));
259

260
            printf ("%10.3lf frames %10.3lf ms total roundtrip latency\n\textra loopback latency: %u frames\n\tuse %u for the backend arguments -I and -O", mtdm->_del, mtdm->_del * t, 
261
                    systemic_latency, systemic_latency/2);
262
263
            if (mtdm->_err > 0.2) printf (" ??");
                if (mtdm->_inv) printf (" Inv");
sletz's avatar
sletz committed
264
265
            printf ("\n");
        }
sletz's avatar
sletz committed
266
267
268
269
270
    }

    return 0;
}

271
// --------------------------------------------------------------------------------