1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2014 Hans Petter Selasky
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27
28 #include <sys/queue.h>
29
30 #include <stdint.h>
31 #include <stdlib.h>
32 #include <string.h>
33 #include <fcntl.h>
34 #include <unistd.h>
35 #include <err.h>
36 #include <math.h>
37 #include <sysexits.h>
38
39 #include "int.h"
40
41 #define REF_FREQ 500 /* HZ */
42
43 uint32_t voss_ad_last_delay;
44 uint8_t voss_ad_enabled;
45 uint8_t voss_ad_output_signal;
46 uint8_t voss_ad_input_channel;
47 uint8_t voss_ad_output_channel;
48
49 static struct voss_ad {
50 double *wave;
51
52 double *sin_a;
53 double *cos_a;
54
55 double *sin_b;
56 double *cos_b;
57
58 double *buf_a;
59 double *buf_b;
60
61 double sum_sin_a;
62 double sum_cos_a;
63
64 double sum_sin_b;
65 double sum_cos_b;
66
67 uint32_t len_a;
68 uint32_t len_b;
69
70 uint32_t offset_a;
71 uint32_t offset_b;
72 } voss_ad;
73
74 void
voss_ad_reset(void)75 voss_ad_reset(void)
76 {
77 uint32_t x;
78
79 for (x = 0; x != voss_ad.len_a; x++)
80 voss_ad.buf_a[x] = 0;
81
82 for (x = 0; x != voss_ad.len_b; x++)
83 voss_ad.buf_b[x] = 0;
84
85 voss_ad.sum_sin_a = 0;
86 voss_ad.sum_cos_a = 0;
87 voss_ad.sum_sin_b = 0;
88 voss_ad.sum_cos_b = 0;
89
90 voss_ad.offset_a = 0;
91 voss_ad.offset_b = 0;
92
93 voss_ad_last_delay = 0;
94 }
95
96 void
voss_ad_init(uint32_t rate)97 voss_ad_init(uint32_t rate)
98 {
99 double freq;
100 int samples;
101 int len;
102 int x;
103
104 len = sqrt(rate);
105
106 samples = len * len;
107
108 voss_ad.wave = malloc(sizeof(voss_ad.wave[0]) * samples);
109
110 voss_ad.sin_a = malloc(sizeof(voss_ad.sin_a[0]) * len);
111 voss_ad.cos_a = malloc(sizeof(voss_ad.cos_a[0]) * len);
112 voss_ad.buf_a = malloc(sizeof(voss_ad.buf_a[0]) * len);
113 voss_ad.len_a = len;
114
115 voss_ad.sin_b = malloc(sizeof(voss_ad.sin_b[0]) * samples);
116 voss_ad.cos_b = malloc(sizeof(voss_ad.cos_b[0]) * samples);
117 voss_ad.buf_b = malloc(sizeof(voss_ad.buf_b[0]) * samples);
118 voss_ad.len_b = samples;
119
120 if (voss_ad.sin_a == NULL || voss_ad.cos_a == NULL ||
121 voss_ad.sin_b == NULL || voss_ad.cos_b == NULL ||
122 voss_ad.buf_a == NULL || voss_ad.buf_b == NULL)
123 errx(EX_SOFTWARE, "Out of memory");
124
125 freq = 1.0;
126
127 while (1) {
128 double temp = freq * ((double)rate) / ((double)len);
129 if (temp >= REF_FREQ)
130 break;
131 freq += 1.0;
132 }
133
134 for (x = 0; x != len; x++) {
135 voss_ad.sin_a[x] = sin(freq * 2.0 * M_PI * ((double)x) / ((double)len));
136 voss_ad.cos_a[x] = cos(freq * 2.0 * M_PI * ((double)x) / ((double)len));
137 voss_ad.buf_a[x] = 0;
138 }
139
140 for (x = 0; x != samples; x++) {
141
142 voss_ad.wave[x] = sin(freq * 2.0 * M_PI * ((double)x) / ((double)len)) *
143 (1.0 + sin(2.0 * M_PI * ((double)x) / ((double)samples))) / 2.0;
144
145 voss_ad.sin_b[x] = sin(2.0 * M_PI * ((double)x) / ((double)samples));
146 voss_ad.cos_b[x] = cos(2.0 * M_PI * ((double)x) / ((double)samples));
147 voss_ad.buf_b[x] = 0;
148 }
149 }
150
151 static double
voss_add_decode_offset(double x,double y)152 voss_add_decode_offset(double x /* cos */, double y /* sin */)
153 {
154 uint32_t v;
155 double r;
156
157 r = sqrt((x * x) + (y * y));
158
159 if (r == 0.0)
160 return (0);
161
162 x /= r;
163 y /= r;
164
165 v = 0;
166
167 if (y < 0) {
168 v |= 1;
169 y = -y;
170 }
171 if (x < 0) {
172 v |= 2;
173 x = -x;
174 }
175
176 if (y < x) {
177 r = acos(y);
178 } else {
179 r = asin(x);
180 }
181
182 switch (v) {
183 case 0:
184 r = (2.0 * M_PI) - r;
185 break;
186 case 1:
187 r = M_PI + r;
188 break;
189 case 3:
190 r = M_PI - r;
191 break;
192 default:
193 break;
194 }
195 return (r);
196 }
197
198 double
voss_ad_getput_sample(double sample)199 voss_ad_getput_sample(double sample)
200 {
201 double retval;
202 double phase;
203 uint32_t xa;
204 uint32_t xb;
205
206 xa = voss_ad.offset_a;
207 xb = voss_ad.offset_b;
208 retval = voss_ad.wave[xb];
209
210 sample -= voss_ad.buf_a[xa];
211 voss_ad.sum_sin_a += voss_ad.sin_a[xa] * sample;
212 voss_ad.sum_cos_a += voss_ad.cos_a[xa] * sample;
213 voss_ad.buf_a[xa] += sample;
214
215 sample = sqrt((voss_ad.sum_sin_a * voss_ad.sum_sin_a) +
216 (voss_ad.sum_cos_a * voss_ad.sum_cos_a));
217
218 sample -= voss_ad.buf_b[xb];
219 voss_ad.sum_sin_b += voss_ad.sin_b[xb] * sample;
220 voss_ad.sum_cos_b += voss_ad.cos_b[xb] * sample;
221 voss_ad.buf_b[xb] += sample;
222
223 if (++xa == voss_ad.len_a)
224 xa = 0;
225
226 if (++xb == voss_ad.len_b) {
227 xb = 0;
228
229 phase = voss_add_decode_offset(
230 voss_ad.sum_cos_b, voss_ad.sum_sin_b);
231
232 voss_ad_last_delay = (uint32_t)(phase * (double)(voss_ad.len_b) / (2.0 * M_PI)) - (voss_ad.len_a / 2);
233 if (voss_ad_last_delay > voss_ad.len_b)
234 voss_ad_last_delay = voss_ad.len_b;
235 }
236 voss_ad.offset_a = xa;
237 voss_ad.offset_b = xb;
238
239 return (retval * (1LL << voss_ad_output_signal));
240 }
241