/*-
 * Copyright (c) 2008 Ariff Abdullah <ariff@FreeBSD.org>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#ifdef _KERNEL
#include <dev/sound/pcm/sound.h>
#include <dev/sound/pcm/pcm.h>
#include "feeder_if.h"

#define SND_USE_FXDIV
#include "snd_fxdiv_gen.h"

SND_DECLARE_FILE("$FreeBSD$");
#endif

#define FEEDMATRIX_RESERVOIR		(SND_CHN_MAX * PCM_32_BPS)

struct feed_matrix_info;

typedef void (*feed_matrix_t)(struct feed_matrix_info *, uint8_t *,
    uint8_t *, uint32_t);

struct feed_matrix_info {
	uint32_t bps;
	uint32_t ialign, oalign;
	feed_matrix_t convert;
	uint8_t reservoir[FEEDMATRIX_RESERVOIR];
};

static void
feed_matrix_stereotomono(struct feed_matrix_info *info,
    uint8_t *src, uint8_t *dst, uint32_t count)
{
	uint32_t i;

	do {
		i = info->bps;

		do {
			*dst++ = *src++;
		} while (--i != 0);

		src += info->bps;

	} while (--count != 0);
}

static void
feed_matrix_monotostereo(struct feed_matrix_info *info,
    uint8_t *src, uint8_t *dst, uint32_t count)
{
	uint32_t i, j;
	uint8_t v;


	do {
		i = info->bps;
		j = i << 1;

		do {
			v = src[--i];
			dst[--j] = v;
			dst[j - info->bps] = v;
		} while (i != 0);

		src += info->ialign;
		dst += info->oalign;

	} while (--count != 0);
}

static void
feed_matrix_swapstereo(struct feed_matrix_info *info,
    uint8_t *src, uint8_t *dst, uint32_t count)
{
	uint32_t i;
	uint8_t v;

	do {
		i = info->bps;

		do {
			v = src[--i];
			dst[i] = src[info->bps + i];
			dst[info->bps + i] = v;
		} while (i != 0);

		src += info->ialign;
		dst += info->oalign;

	} while (--count != 0);
}

static int
feed_matrix_init(struct pcm_feeder *f)
{
	struct feed_matrix_info *info;

	if ((f->desc->in & ~(f->desc->out | AFMT_STEREO)) != 0)
		return (EINVAL);

	info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO);
	if (info == NULL)
		return (ENOMEM);

	info->bps = AFMT_BPS(f->desc->in);

	info->ialign = info->bps * AFMT_CHANNEL(f->desc->in);
	info->oalign = info->bps * AFMT_CHANNEL(f->desc->out);

	if (info->bps == info->ialign && info->oalign > info->ialign)
		info->convert = feed_matrix_monotostereo;
	else if (info->bps == info->oalign && info->ialign > info->oalign)
		info->convert = feed_matrix_stereotomono;
	else if (info->ialign > info->bps && info->ialign == info->oalign)
		info->convert = feed_matrix_swapstereo;
	else
		info->convert = NULL;

	f->data = info;

	return (0);
}

static int
feed_matrix_free(struct pcm_feeder *f)
{
	struct feed_matrix_info *info;

	info = f->data;
	if (info != NULL)
		free(info, M_DEVBUF);

	f->data = NULL;

	return (0);
}

static int
feed_matrix_set(struct pcm_feeder *f, int what, int value)
{
	struct feed_matrix_info *info;

	if (what != FEEDMATRIX_TYPE)
		return (EINVAL);

	info = f->data;

	switch (value) {
	case FEEDMATRIX_TYPE_STEREO_TO_MONO:
		info->ialign = info->bps << 1;
		info->oalign = info->bps;
		info->convert = feed_matrix_stereotomono;
		break;
	case FEEDMATRIX_TYPE_MONO_TO_STEREO:
		info->ialign = info->bps;
		info->oalign = info->bps << 1;
		info->convert = feed_matrix_monotostereo;
		break;
	case FEEDMATRIX_TYPE_SWAP_STEREO:
		info->ialign = info->bps << 1;
		info->oalign = info->bps << 1;
		info->convert = feed_matrix_swapstereo;
		break;
	default:
		return (EINVAL);
		break;
	}

	return (0);
}

static int
feed_matrix_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
    uint32_t count, void *source)
{
	struct feed_matrix_info *info;
	uint32_t j;
	uint8_t *src, *dst;

	info = f->data;
	if (info->convert == NULL)
		return (FEEDER_FEED(f->source, c, b, count, source));

	dst = b;
	count = SND_FXROUND(count, info->oalign);

	do {
		if (count < info->oalign)
			break;

		if (count < info->ialign) {
			src = info->reservoir;
			j = info->ialign;
		} else {
			if (info->ialign == info->oalign)
				j = count;
			else if (info->ialign > info->oalign)
				j = SND_FXROUND(count, info->ialign);
			else
				j = SND_FXDIV(count, info->oalign) *
				    info->ialign;
			src = dst + count - j;
		}

		j = SND_FXDIV(FEEDER_FEED(f->source, c, src, j, source),
		    info->ialign);
		if (j == 0)
			break;

		info->convert(info, src, dst, j);

		j *= info->oalign;
		dst += j;
		count -= j;

	} while (count != 0);

	return (dst - b);
}

static struct pcm_feederdesc feeder_matrix_desc[] = {
	{ FEEDER_MATRIX, 0, 0, 0 },
	{ 0, 0, 0, 0 }
};

static kobj_method_t feeder_matrix_methods[] = {
	KOBJMETHOD(feeder_init,		feed_matrix_init),
	KOBJMETHOD(feeder_free,		feed_matrix_free),
	KOBJMETHOD(feeder_set,		feed_matrix_set),
	KOBJMETHOD(feeder_feed,		feed_matrix_feed),
	{ 0, 0 }
};

FEEDER_DECLARE(feeder_matrix, 0, NULL);