1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2021 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 <string.h>
32 #include <stdlib.h>
33 #include <errno.h>
34
35 #include "int.h"
36
37 void
vclient_tx_equalizer(struct virtual_client * pvc,int64_t * src,size_t total)38 vclient_tx_equalizer(struct virtual_client *pvc,
39 int64_t *src, size_t total)
40 {
41 double *f_data;
42 size_t channels;
43 size_t f_size;
44 size_t x;
45
46 f_size = pvc->profile->tx_filter_size;
47 if (f_size == 0 || total == 0)
48 return;
49
50 channels = pvc->channels;
51 total /= channels;
52
53 while (1) {
54 size_t delta;
55 size_t offset;
56 size_t y;
57
58 offset = pvc->tx_filter_offset;
59 delta = f_size - offset;
60
61 if (delta > total)
62 delta = total;
63
64 for (x = 0; x != channels; x++) {
65 f_data = pvc->profile->tx_filter_data[x];
66 if (f_data == NULL)
67 continue;
68
69 for (y = 0; y != delta; y++) {
70 pvc->tx_filter_in[x][y + offset] = src[x + y * channels];
71 src[x + y * channels] = pvc->tx_filter_out[x][y + offset];
72 }
73 }
74
75 pvc->tx_filter_offset += delta;
76 total -= delta;
77 src += delta * channels;
78
79 /* check if there is enough data for a new transform */
80 if (pvc->tx_filter_offset == f_size) {
81 for (x = 0; x != channels; x++) {
82 f_data = pvc->profile->tx_filter_data[x];
83 if (f_data == NULL)
84 continue;
85
86 /* shift down output */
87 for (y = 0; y != f_size; y++) {
88 pvc->tx_filter_out[x][y] = pvc->tx_filter_out[x][y + f_size];
89 pvc->tx_filter_out[x][y + f_size] = 0;
90 }
91 /* perform transform */
92 voss_x3_multiply_double(pvc->tx_filter_in[x],
93 f_data, pvc->tx_filter_out[x], f_size);
94 }
95 pvc->tx_filter_offset = 0;
96 }
97 if (total == 0)
98 break;
99 }
100 }
101
102 void
vclient_rx_equalizer(struct virtual_client * pvc,int64_t * src,size_t total)103 vclient_rx_equalizer(struct virtual_client *pvc,
104 int64_t *src, size_t total)
105 {
106 double *f_data;
107 size_t channels;
108 size_t f_size;
109 size_t x;
110
111 f_size = pvc->profile->rx_filter_size;
112
113 if (f_size == 0 || total == 0)
114 return;
115
116 channels = pvc->channels;
117 total /= channels;
118
119 while (1) {
120 size_t delta;
121 size_t offset;
122 size_t y;
123
124 offset = pvc->rx_filter_offset;
125 delta = f_size - offset;
126
127 if (delta > total)
128 delta = total;
129
130 for (x = 0; x != channels; x++) {
131 f_data = pvc->profile->rx_filter_data[x];
132 if (f_data == NULL)
133 continue;
134
135 for (y = 0; y != delta; y++) {
136 pvc->rx_filter_in[x][y + offset] = src[x + y * channels];
137 src[x + y * channels] = pvc->rx_filter_out[x][y + offset];
138 }
139 }
140
141 pvc->rx_filter_offset += delta;
142 total -= delta;
143 src += delta * channels;
144
145 /* check if there is enough data for a new transform */
146 if (pvc->rx_filter_offset == f_size) {
147 for (x = 0; x != channels; x++) {
148 f_data = pvc->profile->rx_filter_data[x];
149 if (f_data == NULL)
150 continue;
151
152 /* shift output down */
153 for (y = 0; y != f_size; y++) {
154 pvc->rx_filter_out[x][y] = pvc->rx_filter_out[x][y + f_size];
155 pvc->rx_filter_out[x][y + f_size] = 0;
156 }
157 /* perform transform */
158 voss_x3_multiply_double(pvc->rx_filter_in[x],
159 f_data, pvc->rx_filter_out[x], f_size);
160 }
161 pvc->rx_filter_offset = 0;
162 }
163 if (total == 0)
164 break;
165 }
166 }
167
168 int
vclient_eq_alloc(struct virtual_client * pvc)169 vclient_eq_alloc(struct virtual_client *pvc)
170 {
171 uint8_t x;
172
173 pvc->tx_filter_offset = 0;
174 pvc->rx_filter_offset = 0;
175
176 for (x = 0; x != pvc->channels; x++) {
177 uint32_t size;
178
179 size = pvc->profile->tx_filter_size;
180 if (size != 0) {
181 pvc->tx_filter_in[x] =
182 malloc(sizeof(pvc->tx_filter_in[x][0]) * size);
183 pvc->tx_filter_out[x] =
184 calloc(2 * size, sizeof(pvc->tx_filter_out[x][0]));
185 if (pvc->tx_filter_in[x] == NULL ||
186 pvc->tx_filter_out[x] == NULL)
187 goto error;
188 }
189 size = pvc->profile->rx_filter_size;
190 if (size != 0) {
191 pvc->rx_filter_in[x] =
192 malloc(sizeof(pvc->rx_filter_in[x][0]) * size);
193 pvc->rx_filter_out[x] =
194 calloc(2 * size, sizeof(pvc->rx_filter_out[x][0]));
195 if (pvc->rx_filter_in[x] == NULL ||
196 pvc->rx_filter_out[x] == NULL)
197 goto error;
198 }
199 }
200 return (0);
201
202 error:
203 vclient_eq_free(pvc);
204 return (ENOMEM);
205 }
206
207 void
vclient_eq_free(struct virtual_client * pvc)208 vclient_eq_free(struct virtual_client *pvc)
209 {
210 uint8_t x;
211
212 pvc->tx_filter_offset = 0;
213 pvc->rx_filter_offset = 0;
214
215 for (x = 0; x != VMAX_CHAN; x++) {
216 free(pvc->tx_filter_in[x]);
217 pvc->tx_filter_in[x] = NULL;
218
219 free(pvc->rx_filter_in[x]);
220 pvc->rx_filter_in[x] = NULL;
221
222 free(pvc->tx_filter_out[x]);
223 pvc->tx_filter_out[x] = NULL;
224
225 free(pvc->rx_filter_out[x]);
226 pvc->rx_filter_out[x] = NULL;
227 }
228 }
229