audio_grc3.c revision 9484:fbd5ddc28e96 
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
37extern const int32_t filter_data_L[];
38extern const int32_t filter_data_M[];
39extern const int32_t filter_data_H[];
40extern const int32_t filter_data_P[];
41
42#define	filter_data_HX  filter_data_H
43#define	filter_data_PX  filter_data_P
44
45static int32_t
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
53static int32_t
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
60static int32_t
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)						\
69static 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)						\
77static 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
90DEFINE_FILTER(L)
91DEFINE_FILTER(M)
92DEFINE_FILTER(H)
93DEFINE_FILTER_HQ(HX)
94DEFINE_FILTER(P)
95DEFINE_FILTER_HQ(PX)
96
97#define	DEFINE_CONVD(T, SZ)						\
98static 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
114DEFINE_CONVD(L, 4096)
115DEFINE_CONVD(M, 8192)
116DEFINE_CONVD(H, 16384)
117DEFINE_CONVD(HX, 16384)
118DEFINE_CONVD(P, 32768)
119DEFINE_CONVD(PX, 32768)
120
121static 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
136static int32_t
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
146static int32_t
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
158static int32_t
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
170static int32_t
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
186static int32_t
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)						\
203static void								\
204grc3_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	}								\
239endloop:								\
240									\
241	grc->ptr = ptr;							\
242	grc->historyptr = history;					\
243	grc->outsz = dstsz;						\
244}									\
245									\
246static void								\
247grc3_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	}								\
289endloop:								\
290									\
291	grc->ptr = ptr;							\
292	grc->historyptr = history;					\
293	grc->outsz = dstsz;						\
294}									\
295									\
296static void								\
297grc3_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
308GRC3_RESAMPLE(L)
309GRC3_RESAMPLE(M)
310GRC3_RESAMPLE(H)
311GRC3_RESAMPLE(HX)
312GRC3_RESAMPLE(P)
313GRC3_RESAMPLE(PX)
314
315/*
316 * For performance reasons, we only support 24-bit SRC.
317 */
318void
319grc3_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
347void
348grc3_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
358static void
359grc3_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
366static void
367grc3_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
376void
377grc3_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