1 /*
2  * CDDL HEADER START
3  *
4  * The contents of this file are subject to the terms of the
5  * Common Development and Distribution License (the "License").
6  * You may not use this file except in compliance with the License.
7  *
8  * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
9  * or http://www.opensolaris.org/os/licensing.
10  * See the License for the specific language governing permissions
11  * and limitations under the License.
12  *
13  * When distributing Covered Code, include this CDDL HEADER in each
14  * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
15  * If applicable, add the following below this CDDL HEADER, with the
16  * fields enclosed by brackets "[]" replaced with your own identifying
17  * information: Portions Copyright [yyyy] [name of copyright owner]
18  *
19  * CDDL HEADER END
20  */
21 /*
22  * Copyright (C) 4Front Technologies 1996-2008.
23  *
24  * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
25  * Use is subject to license terms.
26  */
27 
28 /*
29  * Purpose: GRC3 Sample Rate Converter
30  *
31  * GRC library version 3.1
32  */
33 
34 #include <sys/types.h>
35 #include "audio_grc3.h"
36 
37 extern const int32_t filter_data_L[];
38 extern const int32_t filter_data_M[];
39 extern const int32_t filter_data_H[];
40 extern const int32_t filter_data_P[];
41 
42 #define	filter_data_HX  filter_data_H
43 #define	filter_data_PX  filter_data_P
44 
45 static int32_t
_muldivu64(uint32_t a,uint32_t val1,uint32_t val2)46 _muldivu64(uint32_t a, uint32_t val1, uint32_t val2)
47 {
48 	uint64_t v = ((uint64_t)a) * val1 / val2;
49 	return ((uint32_t)(v));
50 }
51 
52 
53 static int32_t
_grc_sat6(int32_t a,int32_t b)54 _grc_sat6(int32_t a, int32_t b)
55 {
56 	int64_t v = ((int64_t)a) * b + (1 << 5);
57 	return ((int32_t)(v >> 6));
58 }
59 
60 static int32_t
_grc_sat31(int32_t a,int32_t b)61 _grc_sat31(int32_t a, int32_t b)
62 {
63 	int64_t v = ((int64_t)a) * b + (1 << 30);
64 	return ((int32_t)(v >> 31));
65 }
66 
67 
68 #define	DEFINE_FILTER(T)						\
69 static int32_t								\
70 _filt31_##T(int32_t a, int32_t idx)					\
71 {									\
72 	int64_t v = ((int64_t)a) * filter_data_##T[idx >> 15];		\
73 	return ((int32_t)(v >> 31));					\
74 }
75 
76 #define	DEFINE_FILTER_HQ(T)						\
77 static int32_t								\
78 _filt31_##T(int32_t a, int32_t idx)					\
79 {									\
80 	int32_t idx2 = idx>>15;						\
81 	int64_t v = ((int64_t)a) *					\
82 									\
83 	    (filter_data_##T[idx2] +					\
84 	    (((int64_t)(idx & 32767)) * (filter_data_##T[idx2 + 1] -	\
85 	    filter_data_##T[idx2]) >> 15));				\
86 	return ((int32_t)(v>>31));					\
87 }
88 
89 
90 DEFINE_FILTER(L)
DEFINE_FILTER(M)91 DEFINE_FILTER(M)
92 DEFINE_FILTER(H)
93 DEFINE_FILTER_HQ(HX)
94 DEFINE_FILTER(P)
95 DEFINE_FILTER_HQ(PX)
96 
97 #define	DEFINE_CONVD(T, SZ)						\
98 static int32_t								\
99 _conv31d_##T(int32_t *history,  uint32_t filter, uint32_t incv)		\
100 {									\
101 	int32_t accum = 0;						\
102 									\
103 	filter = (1024 << 15) - filter;					\
104 									\
105 	while (filter < ((uint32_t)(SZ << 15))) {			\
106 		accum += _filt31_##T(*history, filter);			\
107 		filter += incv;						\
108 		history--;						\
109 	}								\
110 									\
111 	return (accum);							\
112 }
113 
114 DEFINE_CONVD(L, 4096)
115 DEFINE_CONVD(M, 8192)
116 DEFINE_CONVD(H, 16384)
117 DEFINE_CONVD(HX, 16384)
118 DEFINE_CONVD(P, 32768)
119 DEFINE_CONVD(PX, 32768)
120 
121 static int32_t
122 _conv31_L(int32_t *history, uint32_t filter)
123 {
124 	int32_t accum = 0;
125 
126 #define	ITERATION(p)				\
127 	accum += _filt31_##p(*history, filter);	\
128 	filter += (1024 << 15);			\
129 	history--
130 
131 	ITERATION(L); ITERATION(L); ITERATION(L); ITERATION(L);
132 	return (accum);
133 }
134 
135 
136 static int32_t
_conv31_M(int32_t * history,uint32_t filter)137 _conv31_M(int32_t *history, uint32_t filter)
138 {
139 	int32_t accum = 0;
140 
141 	ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M);
142 	ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M);
143 	return (accum);
144 }
145 
146 static int32_t
_conv31_H(int32_t * history,uint32_t filter)147 _conv31_H(int32_t *history, uint32_t filter)
148 {
149 	int32_t accum = 0;
150 
151 	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
152 	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
153 	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
154 	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
155 	return (accum);
156 }
157 
158 static int32_t
_conv31_HX(int32_t * history,uint32_t filter)159 _conv31_HX(int32_t *history, uint32_t filter)
160 {
161 	int32_t accum = 0;
162 
163 	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
164 	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
165 	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
166 	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
167 	return (accum);
168 }
169 
170 static int32_t
_conv31_P(int32_t * history,uint32_t filter)171 _conv31_P(int32_t *history, uint32_t filter)
172 {
173 	int32_t accum = 0;
174 
175 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
176 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
177 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
178 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
179 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
180 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
181 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
182 	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
183 	return (accum);
184 }
185 
186 static int32_t
_conv31_PX(int32_t * history,uint32_t filter)187 _conv31_PX(int32_t *history, uint32_t filter)
188 {
189 	int32_t accum = 0;
190 
191 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
192 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
193 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
194 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
195 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
196 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
197 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
198 	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
199 	return (accum);
200 }
201 
202 #define	GRC3_RESAMPLE(QUAL)						\
203 static void								\
204 grc3_upsample_##QUAL(grc3state_t *grc, const int32_t *src,		\
205     int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset)	\
206 {									\
207 	int32_t ptr = grc->ptr;						\
208 	int32_t srcrate = grc->srcrate;					\
209 	int32_t dstrate = grc->dstrate;					\
210 	int32_t *history = grc->historyptr;				\
211 	int32_t filtfactor = grc->filtfactor;				\
212 	uint32_t dstsz = 0;						\
213 									\
214 	src += offset;							\
215 	dst += offset;							\
216 									\
217 	while (sz > 0) {						\
218 		while (ptr < dstrate) {					\
219 			if (dstsz >= bufsz)				\
220 				goto endloop;				\
221 			dst[0] = (_conv31_##QUAL(history,		\
222 				_grc_sat6(ptr, filtfactor)));		\
223 			ptr += srcrate;					\
224 			dst += inc;					\
225 			dstsz++;					\
226 		}							\
227 									\
228 		history++;						\
229 		if (history >= (grc->history + GRC3_MAXHISTORY * 2))	\
230 			history -= GRC3_MAXHISTORY;			\
231 									\
232 		history[0] = history[-GRC3_MAXHISTORY] = (*src);	\
233 									\
234 		ptr -= dstrate;						\
235 									\
236 		sz--;							\
237 		src += inc;						\
238 	}								\
239 endloop:								\
240 									\
241 	grc->ptr = ptr;							\
242 	grc->historyptr = history;					\
243 	grc->outsz = dstsz;						\
244 }									\
245 									\
246 static void								\
247 grc3_dnsample_##QUAL(grc3state_t *grc, const int32_t *src,		\
248     int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset)	\
249 {									\
250 	int32_t ptr = grc->ptr;						\
251 	int32_t srcrate = grc->srcrate;					\
252 	int32_t dstrate = grc->dstrate;					\
253 	int32_t sat = grc->sat;						\
254 	int32_t *history = grc->historyptr;				\
255 	int32_t filtfactor = grc->filtfactor;				\
256 	uint32_t dstsz = 0;						\
257 									\
258 	src += offset;							\
259 	dst += offset;							\
260 									\
261 	while (sz > 0) {						\
262 		while (ptr >= srcrate) {				\
263 			if (dstsz >= bufsz)				\
264 				goto endloop;				\
265 			ptr -= srcrate;					\
266 			dst[0] = (_conv31d_##QUAL(history,		\
267 			    _grc_sat6(ptr, filtfactor),			\
268 				grc->ptr_incv));			\
269 			dst += inc;					\
270 			dstsz++;					\
271 		}							\
272 									\
273 		history++;						\
274 		if (history >= (grc->history + GRC3_MAXHISTORY * 2))	\
275 			history -= GRC3_MAXHISTORY;			\
276 									\
277 		/*							\
278 		 * TODO: for better quality multiplier is worth moving	\
279 		 * to output cascade					\
280 		 */							\
281 		history[0] = history[-GRC3_MAXHISTORY] =		\
282 		    _grc_sat31((*src), sat);				\
283 									\
284 		ptr += dstrate;						\
285 									\
286 		sz--;							\
287 		src += inc;						\
288 	}								\
289 endloop:								\
290 									\
291 	grc->ptr = ptr;							\
292 	grc->historyptr = history;					\
293 	grc->outsz = dstsz;						\
294 }									\
295 									\
296 static void								\
297 grc3_resample_##QUAL(grc3state_t *grc, const void *src, void *dst,	\
298     uint32_t sz, uint32_t bufsz, int inc, int  offset)			\
299 {									\
300 	if (grc->srcrate <= grc->dstrate)				\
301 		grc3_upsample_##QUAL(grc, src, dst, sz,			\
302 		    bufsz, inc, offset);				\
303 	else								\
304 		grc3_dnsample_##QUAL(grc, src, dst, sz,			\
305 		    bufsz, inc, offset);				\
306 }
307 
308 GRC3_RESAMPLE(L)
GRC3_RESAMPLE(M)309 GRC3_RESAMPLE(M)
310 GRC3_RESAMPLE(H)
311 GRC3_RESAMPLE(HX)
312 GRC3_RESAMPLE(P)
313 GRC3_RESAMPLE(PX)
314 
315 /*
316  * For performance reasons, we only support 24-bit SRC.
317  */
318 void
319 grc3_convert(grc3state_t *grc, int quality, const void *src,
320     void *dst, int sz, int bufsz, int inc, int offset)
321 {
322 
323 	switch (quality) {
324 	default:
325 	case 0:
326 	case 1:
327 		grc3_resample_L(grc, src, dst, sz, bufsz, inc, offset);
328 		break;
329 	case 2:
330 		grc3_resample_M(grc, src, dst, sz, bufsz, inc, offset);
331 		break;
332 	case 3:
333 		grc3_resample_H(grc, src, dst, sz, bufsz, inc, offset);
334 		break;
335 	case 4:
336 		grc3_resample_HX(grc, src, dst, sz, bufsz, inc, offset);
337 		break;
338 	case 5:
339 		grc3_resample_P(grc, src, dst, sz, bufsz, inc, offset);
340 		break;
341 	case 6:
342 		grc3_resample_PX(grc, src, dst, sz, bufsz, inc, offset);
343 		break;
344 	}
345 }
346 
347 void
grc3_reset(grc3state_t * grc)348 grc3_reset(grc3state_t *grc)
349 {
350 	int32_t t;
351 	grc->ptr = 0;
352 	grc->historyptr = grc->history + GRC3_MAXHISTORY;
353 
354 	for (t = 0; t < GRC3_MAXHISTORY * 2; t++)
355 		grc->history[t] = 0;
356 }
357 
358 static void
grc3_setup_up(grc3state_t * grc,uint32_t fromRate,uint32_t toRate)359 grc3_setup_up(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
360 {
361 	grc->srcrate = fromRate;
362 	grc->dstrate = toRate;
363 	grc->filtfactor = 0x80000000U / toRate;
364 }
365 
366 static void
grc3_setup_dn(grc3state_t * grc,uint32_t fromRate,uint32_t toRate)367 grc3_setup_dn(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
368 {
369 	grc->srcrate = fromRate;
370 	grc->dstrate = toRate;
371 	grc->filtfactor = 0x80000000U / fromRate;
372 	grc->ptr_incv = _muldivu64(1024 << 15, toRate, fromRate);
373 	grc->sat = _muldivu64(0x80000000U, toRate, fromRate);
374 }
375 
376 void
grc3_setup(grc3state_t * grc,uint32_t fromRate,uint32_t toRate)377 grc3_setup(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
378 {
379 	while ((!(fromRate & 1)) && (!(toRate & 1)) && (fromRate > 0)) {
380 		fromRate >>= 1;
381 		toRate >>= 1;
382 	}
383 
384 	if (fromRate <= toRate)
385 		grc3_setup_up(grc, fromRate, toRate);
386 	else
387 		grc3_setup_dn(grc, fromRate, toRate);
388 }
389