xref: /illumos-gate/usr/src/uts/common/io/audio/impl/audio_grc3.c (revision 88447a05)

/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License (the "License").
 * You may not use this file except in compliance with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */
/*
 * Copyright (C) 4Front Technologies 1996-2008.
 *
 * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */

/*
 * Purpose: GRC3 Sample Rate Converter
 *
 * GRC library version 3.1
 */

#include <sys/types.h>
#include "audio_grc3.h"

extern const int32_t filter_data_L[];
extern const int32_t filter_data_M[];
extern const int32_t filter_data_H[];
extern const int32_t filter_data_P[];

#define	filter_data_HX  filter_data_H
#define	filter_data_PX  filter_data_P

static int32_t
_muldivu64(uint32_t a, uint32_t val1, uint32_t val2)
{
	uint64_t v = ((uint64_t)a) * val1 / val2;
	return ((uint32_t)(v));
}


static int32_t
_grc_sat6(int32_t a, int32_t b)
{
	int64_t v = ((int64_t)a) * b + (1 << 5);
	return ((int32_t)(v >> 6));
}

static int32_t
_grc_sat31(int32_t a, int32_t b)
{
	int64_t v = ((int64_t)a) * b + (1 << 30);
	return ((int32_t)(v >> 31));
}


#define	DEFINE_FILTER(T)						\
static int32_t								\
_filt31_##T(int32_t a, int32_t idx)					\
{									\
	int64_t v = ((int64_t)a) * filter_data_##T[idx >> 15];		\
	return ((int32_t)(v >> 31));					\
}

#define	DEFINE_FILTER_HQ(T)						\
static int32_t								\
_filt31_##T(int32_t a, int32_t idx)					\
{									\
	int32_t idx2 = idx>>15;						\
	int64_t v = ((int64_t)a) *					\
									\
	    (filter_data_##T[idx2] +					\
	    (((int64_t)(idx & 32767)) * (filter_data_##T[idx2 + 1] -	\
	    filter_data_##T[idx2]) >> 15));				\
	return ((int32_t)(v>>31));					\
}


DEFINE_FILTER(L)
DEFINE_FILTER(M)
DEFINE_FILTER(H)
DEFINE_FILTER_HQ(HX)
DEFINE_FILTER(P)
DEFINE_FILTER_HQ(PX)

#define	DEFINE_CONVD(T, SZ)						\
static int32_t								\
_conv31d_##T(int32_t *history,  uint32_t filter, uint32_t incv)		\
{									\
	int32_t accum = 0;						\
									\
	filter = (1024 << 15) - filter;					\
									\
	while (filter < ((uint32_t)(SZ << 15))) {			\
		accum += _filt31_##T(*history, filter);			\
		filter += incv;						\
		history--;						\
	}								\
									\
	return (accum);							\
}

DEFINE_CONVD(L, 4096)
DEFINE_CONVD(M, 8192)
DEFINE_CONVD(H, 16384)
DEFINE_CONVD(HX, 16384)
DEFINE_CONVD(P, 32768)
DEFINE_CONVD(PX, 32768)

static int32_t
_conv31_L(int32_t *history, uint32_t filter)
{
	int32_t accum = 0;

#define	ITERATION(p)				\
	accum += _filt31_##p(*history, filter);	\
	filter += (1024 << 15);			\
	history--

	ITERATION(L); ITERATION(L); ITERATION(L); ITERATION(L);
	return (accum);
}


static int32_t
_conv31_M(int32_t *history, uint32_t filter)
{
	int32_t accum = 0;

	ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M);
	ITERATION(M); ITERATION(M); ITERATION(M); ITERATION(M);
	return (accum);
}

static int32_t
_conv31_H(int32_t *history, uint32_t filter)
{
	int32_t accum = 0;

	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
	ITERATION(H); ITERATION(H); ITERATION(H); ITERATION(H);
	return (accum);
}

static int32_t
_conv31_HX(int32_t *history, uint32_t filter)
{
	int32_t accum = 0;

	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
	ITERATION(HX); ITERATION(HX); ITERATION(HX); ITERATION(HX);
	return (accum);
}

static int32_t
_conv31_P(int32_t *history, uint32_t filter)
{
	int32_t accum = 0;

	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	ITERATION(P); ITERATION(P); ITERATION(P); ITERATION(P);
	return (accum);
}

static int32_t
_conv31_PX(int32_t *history, uint32_t filter)
{
	int32_t accum = 0;

	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	ITERATION(PX); ITERATION(PX); ITERATION(PX); ITERATION(PX);
	return (accum);
}

#define	GRC3_RESAMPLE(QUAL)						\
static void								\
grc3_upsample_##QUAL(grc3state_t *grc, const int32_t *src,		\
    int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset)	\
{									\
	int32_t ptr = grc->ptr;						\
	int32_t srcrate = grc->srcrate;					\
	int32_t dstrate = grc->dstrate;					\
	int32_t *history = grc->historyptr;				\
	int32_t filtfactor = grc->filtfactor;				\
	uint32_t dstsz = 0;						\
									\
	src += offset;							\
	dst += offset;							\
									\
	while (sz > 0) {						\
		while (ptr < dstrate) {					\
			if (dstsz >= bufsz)				\
				goto endloop;				\
			dst[0] = (_conv31_##QUAL(history,		\
				_grc_sat6(ptr, filtfactor)));		\
			ptr += srcrate;					\
			dst += inc;					\
			dstsz++;					\
		}							\
									\
		history++;						\
		if (history >= (grc->history + GRC3_MAXHISTORY * 2))	\
			history -= GRC3_MAXHISTORY;			\
									\
		history[0] = history[-GRC3_MAXHISTORY] = (*src);	\
									\
		ptr -= dstrate;						\
									\
		sz--;							\
		src += inc;						\
	}								\
endloop:								\
									\
	grc->ptr = ptr;							\
	grc->historyptr = history;					\
	grc->outsz = dstsz;						\
}									\
									\
static void								\
grc3_dnsample_##QUAL(grc3state_t *grc, const int32_t *src,		\
    int32_t *dst, uint32_t sz, uint32_t bufsz, int inc, int offset)	\
{									\
	int32_t ptr = grc->ptr;						\
	int32_t srcrate = grc->srcrate;					\
	int32_t dstrate = grc->dstrate;					\
	int32_t sat = grc->sat;						\
	int32_t *history = grc->historyptr;				\
	int32_t filtfactor = grc->filtfactor;				\
	uint32_t dstsz = 0;						\
									\
	src += offset;							\
	dst += offset;							\
									\
	while (sz > 0) {						\
		while (ptr >= srcrate) {				\
			if (dstsz >= bufsz)				\
				goto endloop;				\
			ptr -= srcrate;					\
			dst[0] = (_conv31d_##QUAL(history,		\
			    _grc_sat6(ptr, filtfactor),			\
				grc->ptr_incv));			\
			dst += inc;					\
			dstsz++;					\
		}							\
									\
		history++;						\
		if (history >= (grc->history + GRC3_MAXHISTORY * 2))	\
			history -= GRC3_MAXHISTORY;			\
									\
		/*							\
		 * TODO: for better quality multiplier is worth moving	\
		 * to output cascade					\
		 */							\
		history[0] = history[-GRC3_MAXHISTORY] =		\
		    _grc_sat31((*src), sat);				\
									\
		ptr += dstrate;						\
									\
		sz--;							\
		src += inc;						\
	}								\
endloop:								\
									\
	grc->ptr = ptr;							\
	grc->historyptr = history;					\
	grc->outsz = dstsz;						\
}									\
									\
static void								\
grc3_resample_##QUAL(grc3state_t *grc, const void *src, void *dst,	\
    uint32_t sz, uint32_t bufsz, int inc, int  offset)			\
{									\
	if (grc->srcrate <= grc->dstrate)				\
		grc3_upsample_##QUAL(grc, src, dst, sz,			\
		    bufsz, inc, offset);				\
	else								\
		grc3_dnsample_##QUAL(grc, src, dst, sz,			\
		    bufsz, inc, offset);				\
}

GRC3_RESAMPLE(L)
GRC3_RESAMPLE(M)
GRC3_RESAMPLE(H)
GRC3_RESAMPLE(HX)
GRC3_RESAMPLE(P)
GRC3_RESAMPLE(PX)

/*
 * For performance reasons, we only support 24-bit SRC.
 */
void
grc3_convert(grc3state_t *grc, int quality, const void *src,
    void *dst, int sz, int bufsz, int inc, int offset)
{

	switch (quality) {
	default:
	case 0:
	case 1:
		grc3_resample_L(grc, src, dst, sz, bufsz, inc, offset);
		break;
	case 2:
		grc3_resample_M(grc, src, dst, sz, bufsz, inc, offset);
		break;
	case 3:
		grc3_resample_H(grc, src, dst, sz, bufsz, inc, offset);
		break;
	case 4:
		grc3_resample_HX(grc, src, dst, sz, bufsz, inc, offset);
		break;
	case 5:
		grc3_resample_P(grc, src, dst, sz, bufsz, inc, offset);
		break;
	case 6:
		grc3_resample_PX(grc, src, dst, sz, bufsz, inc, offset);
		break;
	}
}

void
grc3_reset(grc3state_t *grc)
{
	int32_t t;
	grc->ptr = 0;
	grc->historyptr = grc->history + GRC3_MAXHISTORY;

	for (t = 0; t < GRC3_MAXHISTORY * 2; t++)
		grc->history[t] = 0;
}

static void
grc3_setup_up(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
{
	grc->srcrate = fromRate;
	grc->dstrate = toRate;
	grc->filtfactor = 0x80000000U / toRate;
}

static void
grc3_setup_dn(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
{
	grc->srcrate = fromRate;
	grc->dstrate = toRate;
	grc->filtfactor = 0x80000000U / fromRate;
	grc->ptr_incv = _muldivu64(1024 << 15, toRate, fromRate);
	grc->sat = _muldivu64(0x80000000U, toRate, fromRate);
}

void
grc3_setup(grc3state_t *grc, uint32_t fromRate, uint32_t toRate)
{
	while ((!(fromRate & 1)) && (!(toRate & 1)) && (fromRate > 0)) {
		fromRate >>= 1;
		toRate >>= 1;
	}

	if (fromRate <= toRate)
		grc3_setup_up(grc, fromRate, toRate);
	else
		grc3_setup_dn(grc, fromRate, toRate);
}