Subversion Repositories or1k_soc_on_altera_embedded_dev_kit

/*

 *  Rate conversion Plug-In

 *  Copyright (c) 1999 by Jaroslav Kysela <perex@perex.cz>

 *

 *

 *   This library is free software; you can redistribute it and/or modify

 *   it under the terms of the GNU Library General Public License as

 *   published by the Free Software Foundation; either version 2 of

 *   the License, or (at your option) any later version.

 *

 *   This program is distributed in the hope that it will be useful,

 *   but WITHOUT ANY WARRANTY; without even the implied warranty of

 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 *   GNU Library General Public License for more details.

 *

 *   You should have received a copy of the GNU Library General Public

 *   License along with this library; if not, write to the Free Software

 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

 *

 */

#include <sound/driver.h>

#include <linux/time.h>

#include <sound/core.h>

#include <sound/pcm.h>

#include "pcm_plugin.h"

#define SHIFT   11

#define BITS    (1<<SHIFT)

#define R_MASK  (BITS-1)

/*

 *  Basic rate conversion plugin

 */

struct rate_channel {

        signed short last_S1;

        signed short last_S2;

};

typedef void (*rate_f)(struct snd_pcm_plugin *plugin,

                       const struct snd_pcm_plugin_channel *src_channels,

                       struct snd_pcm_plugin_channel *dst_channels,

                       int src_frames, int dst_frames);

struct rate_priv {

        unsigned int pitch;

        unsigned int pos;

        rate_f func;

        snd_pcm_sframes_t old_src_frames, old_dst_frames;

        struct rate_channel channels[0];

};

static void rate_init(struct snd_pcm_plugin *plugin)

{

        unsigned int channel;

        struct rate_priv *data = (struct rate_priv *)plugin->extra_data;

        data->pos = 0;

        for (channel = 0; channel < plugin->src_format.channels; channel++) {

                data->channels[channel].last_S1 = 0;

                data->channels[channel].last_S2 = 0;

        }

}

static void resample_expand(struct snd_pcm_plugin *plugin,

                            const struct snd_pcm_plugin_channel *src_channels,

                            struct snd_pcm_plugin_channel *dst_channels,

                            int src_frames, int dst_frames)

{

        unsigned int pos = 0;

        signed int val;

        signed short S1, S2;

        signed short *src, *dst;

        unsigned int channel;

        int src_step, dst_step;

        int src_frames1, dst_frames1;

        struct rate_priv *data = (struct rate_priv *)plugin->extra_data;

        struct rate_channel *rchannels = data->channels;

        for (channel = 0; channel < plugin->src_format.channels; channel++) {

                pos = data->pos;

                S1 = rchannels->last_S1;

                S2 = rchannels->last_S2;

                if (!src_channels[channel].enabled) {

                        if (dst_channels[channel].wanted)

                                snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);

                        dst_channels[channel].enabled = 0;

                        continue;

                }

                dst_channels[channel].enabled = 1;

                src = (signed short *)src_channels[channel].area.addr +

                        src_channels[channel].area.first / 8 / 2;

                dst = (signed short *)dst_channels[channel].area.addr +

                        dst_channels[channel].area.first / 8 / 2;

                src_step = src_channels[channel].area.step / 8 / 2;

                dst_step = dst_channels[channel].area.step / 8 / 2;

                src_frames1 = src_frames;

                dst_frames1 = dst_frames;

                while (dst_frames1-- > 0) {

                        if (pos & ~R_MASK) {

                                pos &= R_MASK;

                                S1 = S2;

                                if (src_frames1-- > 0) {

                                        S2 = *src;

                                        src += src_step;

                                }

                        }

                        val = S1 + ((S2 - S1) * (signed int)pos) / BITS;

                        if (val < -32768)

                                val = -32768;

                        else if (val > 32767)

                                val = 32767;

                        *dst = val;

                        dst += dst_step;

                        pos += data->pitch;

                }

                rchannels->last_S1 = S1;

                rchannels->last_S2 = S2;

                rchannels++;

        }

        data->pos = pos;

}

static void resample_shrink(struct snd_pcm_plugin *plugin,

                            const struct snd_pcm_plugin_channel *src_channels,

                            struct snd_pcm_plugin_channel *dst_channels,

                            int src_frames, int dst_frames)

{

        unsigned int pos = 0;

        signed int val;

        signed short S1, S2;

        signed short *src, *dst;

        unsigned int channel;

        int src_step, dst_step;

        int src_frames1, dst_frames1;

        struct rate_priv *data = (struct rate_priv *)plugin->extra_data;

        struct rate_channel *rchannels = data->channels;

        for (channel = 0; channel < plugin->src_format.channels; ++channel) {

                pos = data->pos;

                S1 = rchannels->last_S1;

                S2 = rchannels->last_S2;

                if (!src_channels[channel].enabled) {

                        if (dst_channels[channel].wanted)

                                snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);

                        dst_channels[channel].enabled = 0;

                        continue;

                }

                dst_channels[channel].enabled = 1;

                src = (signed short *)src_channels[channel].area.addr +

                        src_channels[channel].area.first / 8 / 2;

                dst = (signed short *)dst_channels[channel].area.addr +

                        dst_channels[channel].area.first / 8 / 2;

                src_step = src_channels[channel].area.step / 8 / 2;

                dst_step = dst_channels[channel].area.step / 8 / 2;

                src_frames1 = src_frames;

                dst_frames1 = dst_frames;

                while (dst_frames1 > 0) {

                        S1 = S2;

                        if (src_frames1-- > 0) {

                                S1 = *src;

                                src += src_step;

                        }

                        if (pos & ~R_MASK) {

                                pos &= R_MASK;

                                val = S1 + ((S2 - S1) * (signed int)pos) / BITS;

                                if (val < -32768)

                                        val = -32768;

                                else if (val > 32767)

                                        val = 32767;

                                *dst = val;

                                dst += dst_step;

                                dst_frames1--;

                        }

                        pos += data->pitch;

                }

                rchannels->last_S1 = S1;

                rchannels->last_S2 = S2;

                rchannels++;

        }

        data->pos = pos;

}

static snd_pcm_sframes_t rate_src_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)

{

        struct rate_priv *data;

        snd_pcm_sframes_t res;

        snd_assert(plugin != NULL, return -ENXIO);

        if (frames == 0)

                return 0;

        data = (struct rate_priv *)plugin->extra_data;

        if (plugin->src_format.rate < plugin->dst_format.rate) {

                res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);

        } else {

                res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);

        }

        if (data->old_src_frames > 0) {

                snd_pcm_sframes_t frames1 = frames, res1 = data->old_dst_frames;

                while (data->old_src_frames < frames1) {

                        frames1 >>= 1;

                        res1 <<= 1;

                }

                while (data->old_src_frames > frames1) {

                        frames1 <<= 1;

                        res1 >>= 1;

                }

                if (data->old_src_frames == frames1)

                        return res1;

        }

        data->old_src_frames = frames;

        data->old_dst_frames = res;

        return res;

}

static snd_pcm_sframes_t rate_dst_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)

{

        struct rate_priv *data;

        snd_pcm_sframes_t res;

        snd_assert(plugin != NULL, return -ENXIO);

        if (frames == 0)

                return 0;

        data = (struct rate_priv *)plugin->extra_data;

        if (plugin->src_format.rate < plugin->dst_format.rate) {

                res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);

        } else {

                res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);

        }

        if (data->old_dst_frames > 0) {

                snd_pcm_sframes_t frames1 = frames, res1 = data->old_src_frames;

                while (data->old_dst_frames < frames1) {

                        frames1 >>= 1;

                        res1 <<= 1;

                }

                while (data->old_dst_frames > frames1) {

                        frames1 <<= 1;

                        res1 >>= 1;

                }

                if (data->old_dst_frames == frames1)

                        return res1;

        }

        data->old_dst_frames = frames;

        data->old_src_frames = res;

        return res;

}

static snd_pcm_sframes_t rate_transfer(struct snd_pcm_plugin *plugin,

                             const struct snd_pcm_plugin_channel *src_channels,

                             struct snd_pcm_plugin_channel *dst_channels,

                             snd_pcm_uframes_t frames)

{

        snd_pcm_uframes_t dst_frames;

        struct rate_priv *data;

        snd_assert(plugin != NULL && src_channels != NULL && dst_channels != NULL, return -ENXIO);

        if (frames == 0)

                return 0;

#ifdef CONFIG_SND_DEBUG

        {

                unsigned int channel;

                for (channel = 0; channel < plugin->src_format.channels; channel++) {

                        snd_assert(src_channels[channel].area.first % 8 == 0 &&

                                   src_channels[channel].area.step % 8 == 0,

                                   return -ENXIO);

                        snd_assert(dst_channels[channel].area.first % 8 == 0 &&

                                   dst_channels[channel].area.step % 8 == 0,

                                   return -ENXIO);

                }

        }

#endif

        dst_frames = rate_dst_frames(plugin, frames);

        if (dst_frames > dst_channels[0].frames)

                dst_frames = dst_channels[0].frames;

        data = (struct rate_priv *)plugin->extra_data;

        data->func(plugin, src_channels, dst_channels, frames, dst_frames);

        return dst_frames;

}

static int rate_action(struct snd_pcm_plugin *plugin,

                       enum snd_pcm_plugin_action action,

                       unsigned long udata)

{

        snd_assert(plugin != NULL, return -ENXIO);

        switch (action) {

        case INIT:

        case PREPARE:

                rate_init(plugin);

                break;

        default:

                break;

        }

        return 0;        /* silenty ignore other actions */

}

int snd_pcm_plugin_build_rate(struct snd_pcm_substream *plug,

                              struct snd_pcm_plugin_format *src_format,

                              struct snd_pcm_plugin_format *dst_format,

                              struct snd_pcm_plugin **r_plugin)

{

        int err;

        struct rate_priv *data;

        struct snd_pcm_plugin *plugin;

        snd_assert(r_plugin != NULL, return -ENXIO);

        *r_plugin = NULL;

        snd_assert(src_format->channels == dst_format->channels, return -ENXIO);

        snd_assert(src_format->channels > 0, return -ENXIO);

        snd_assert(src_format->format == SNDRV_PCM_FORMAT_S16, return -ENXIO);

        snd_assert(dst_format->format == SNDRV_PCM_FORMAT_S16, return -ENXIO);

        snd_assert(src_format->rate != dst_format->rate, return -ENXIO);

        err = snd_pcm_plugin_build(plug, "rate conversion",

                                   src_format, dst_format,

                                   sizeof(struct rate_priv) +

                                   src_format->channels * sizeof(struct rate_channel),

                                   &plugin);

        if (err < 0)

                return err;

        data = (struct rate_priv *)plugin->extra_data;

        if (src_format->rate < dst_format->rate) {

                data->pitch = ((src_format->rate << SHIFT) + (dst_format->rate >> 1)) / dst_format->rate;

                data->func = resample_expand;

        } else {

                data->pitch = ((dst_format->rate << SHIFT) + (src_format->rate >> 1)) / src_format->rate;

                data->func = resample_shrink;

        }

        data->pos = 0;

        rate_init(plugin);

        data->old_src_frames = data->old_dst_frames = 0;

        plugin->transfer = rate_transfer;

        plugin->src_frames = rate_src_frames;

        plugin->dst_frames = rate_dst_frames;

        plugin->action = rate_action;

        *r_plugin = plugin;

        return 0;

}