357 lines
9.5 KiB
C
357 lines
9.5 KiB
C
/***************************************************************************
|
|
* __________ __ ___.
|
|
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
|
|
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
|
|
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
|
|
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
|
|
* \/ \/ \/ \/ \/
|
|
* $Id$
|
|
*
|
|
* Copyright (C) 2014 by Chiwen Chang
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License
|
|
* as published by the Free Software Foundation; either version 2
|
|
* of the License, or (at your option) any later version.
|
|
*
|
|
* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
|
|
* KIND, either express or implied.
|
|
*
|
|
****************************************************************************/
|
|
#include "surround.h"
|
|
#include "config.h"
|
|
#include "fixedpoint.h"
|
|
#include "fracmul.h"
|
|
#include "settings.h"
|
|
#include "dsp_proc_entry.h"
|
|
#include "dsp_filter.h"
|
|
#include "core_alloc.h"
|
|
|
|
static int surround_balance = 0;
|
|
static bool surround_side_only = false;
|
|
static int surround_mix = 100;
|
|
static int surround_strength = 0;
|
|
/*1 sample ~ 11ns */
|
|
#define DLY_5MS 454
|
|
#define DLY_8MS 727
|
|
#define DLY_10MS 909
|
|
#define DLY_15MS 1363
|
|
#define DLY_30MS 2727
|
|
#define MAX_DLY DLY_30MS
|
|
|
|
#define B0_DLY (MAX_DLY/8 + 1)
|
|
#define B2_DLY (MAX_DLY + 1)
|
|
#define BB_DLY (MAX_DLY/4 + 1)
|
|
#define HH_DLY (MAX_DLY/2 + 1)
|
|
#define CL_DLY B2_DLY
|
|
|
|
/*voice from 300hz - 3400hz ?*/
|
|
static int32_t tcoef1,tcoef2,bcoef,hcoef;
|
|
|
|
static int dly_size = MAX_DLY;
|
|
static int cutoff_l = 320;
|
|
static int cutoff_h = 3400;
|
|
|
|
static int b0_r=0,b0_w=0,
|
|
b2_r=0,b2_w=0,
|
|
bb_r=0,bb_w=0,
|
|
hh_r=0,hh_w=0,
|
|
cl_r=0,cl_w=0;
|
|
static int handle = -1;
|
|
|
|
#define SURROUND_BUFSIZE ((B0_DLY + B2_DLY + BB_DLY + HH_DLY + CL_DLY)*sizeof (int32_t))
|
|
|
|
static int surround_buffer_alloc(void)
|
|
{
|
|
handle = core_alloc("dsp_surround_buffer", SURROUND_BUFSIZE);
|
|
return handle;
|
|
}
|
|
|
|
static void surround_buffer_free(void)
|
|
{
|
|
if (handle < 0)
|
|
return;
|
|
|
|
core_free(handle);
|
|
handle = -1;
|
|
}
|
|
|
|
static void dsp_surround_flush(void)
|
|
{
|
|
memset(core_get_data(handle), 0, SURROUND_BUFSIZE);
|
|
}
|
|
|
|
static void surround_update_filter(unsigned int fout)
|
|
{
|
|
tcoef1 = fp_div(cutoff_l, fout, 31);
|
|
tcoef2 = fp_div(cutoff_h, fout, 31);
|
|
bcoef = fp_div(cutoff_l / 2, fout, 31);
|
|
hcoef = fp_div(cutoff_h * 2, fout, 31);
|
|
}
|
|
|
|
void dsp_surround_set_balance(int var)
|
|
{
|
|
surround_balance = var;
|
|
}
|
|
|
|
void dsp_surround_side_only(bool var)
|
|
{
|
|
surround_side_only = var;
|
|
}
|
|
|
|
void dsp_surround_mix(int var)
|
|
{
|
|
surround_mix = var;
|
|
}
|
|
|
|
void dsp_surround_set_cutoff(int frq_l, int frq_h)
|
|
{
|
|
if (cutoff_l == frq_l && cutoff_h == frq_h)
|
|
return; /* No settings change */
|
|
|
|
cutoff_l = frq_l;/*fx2*/
|
|
cutoff_h = frq_h;/*fx1*/
|
|
|
|
struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
|
|
if (!dsp_proc_enabled(dsp, DSP_PROC_SURROUND))
|
|
return;
|
|
|
|
surround_update_filter(dsp_get_output_frequency(dsp));
|
|
}
|
|
|
|
static void surround_set_stepsize(int surround_strength)
|
|
{
|
|
if (handle > 0)
|
|
dsp_surround_flush();
|
|
|
|
switch(surround_strength)
|
|
{
|
|
case 1:
|
|
dly_size = DLY_5MS;
|
|
break;
|
|
case 2:
|
|
dly_size = DLY_8MS;
|
|
break;
|
|
case 3:
|
|
dly_size = DLY_10MS;
|
|
break;
|
|
case 4:
|
|
dly_size = DLY_15MS;
|
|
break;
|
|
case 5:
|
|
dly_size = DLY_30MS;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void dsp_surround_enable(int var)
|
|
{
|
|
if (var == surround_strength)
|
|
return; /* No setting change */
|
|
|
|
surround_strength = var;
|
|
|
|
struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
|
|
bool was_enabled = dsp_proc_enabled(dsp, DSP_PROC_SURROUND);
|
|
bool now_enabled = var > 0;
|
|
|
|
if (was_enabled == now_enabled)
|
|
return; /* No change in enabled status */
|
|
|
|
if (now_enabled)
|
|
surround_set_stepsize(var);
|
|
|
|
/* If changing status, enable or disable it; if already enabled push
|
|
additional DSP_PROC_INIT messages with value = 1 to force-update the
|
|
filters */
|
|
dsp_proc_enable(dsp, DSP_PROC_SURROUND, now_enabled);
|
|
}
|
|
|
|
static void surround_process(struct dsp_proc_entry *this,
|
|
struct dsp_buffer **buf_p)
|
|
{
|
|
|
|
struct dsp_buffer *buf = *buf_p;
|
|
int count = buf->remcount;
|
|
|
|
int dly_shift3 = dly_size/8;
|
|
int dly_shift2 = dly_size/4;
|
|
int dly_shift1 = dly_size/2;
|
|
int dly = dly_size;
|
|
int i;
|
|
int32_t x;
|
|
|
|
/*only need to buffer right channel */
|
|
static int32_t *b0, *b2, *bb, *hh, *cl;
|
|
|
|
b0 = core_get_data(handle);
|
|
b2 = b0 + B0_DLY;
|
|
bb = b2 + B2_DLY;
|
|
hh = bb + BB_DLY;
|
|
cl = hh + HH_DLY;
|
|
|
|
for (i = 0; i < count; i++)
|
|
{
|
|
int32_t mid = buf->p32[0][i] / 2 + buf->p32[1][i] / 2;
|
|
int32_t side = buf->p32[0][i] - buf->p32[1][i];
|
|
int32_t temp0, temp1;
|
|
|
|
if (!surround_side_only)
|
|
{
|
|
/*clone the left channal*/
|
|
temp0 = buf->p32[0][i];
|
|
/*keep the middle band of right channel*/
|
|
temp1 = FRACMUL(buf->p32[1][i], tcoef1) -
|
|
FRACMUL(buf->p32[1][i], tcoef2);
|
|
}
|
|
else /* apply haas to side only*/
|
|
{
|
|
temp0 = side / 2;
|
|
temp1 = FRACMUL(-side, tcoef1) / 2 -
|
|
FRACMUL(-side, tcoef2) / 2;
|
|
}
|
|
|
|
/* inverted crossfeed delay (left channel) to make sound wider*/
|
|
x = temp1/100 * 35;
|
|
temp0 += dequeue(cl, &cl_r, dly);
|
|
enqueue(-x, cl, &cl_w, dly);
|
|
|
|
/* apply 1/8 delay to frequency below fx2 */
|
|
x = buf->p32[1][i] - FRACMUL(buf->p32[1][i], tcoef1);
|
|
temp1 += dequeue(b0, &b0_r, dly_shift3);
|
|
enqueue(x, b0, &b0_w, dly_shift3 );
|
|
|
|
/* cut frequency below half fx2*/
|
|
temp1 = FRACMUL(temp1, bcoef);
|
|
|
|
/* apply 1/4 delay to frequency below half fx2 */
|
|
/* use different delay to fake the sound direction*/
|
|
x = buf->p32[1][i] - FRACMUL(buf->p32[1][i], bcoef);
|
|
temp1 += dequeue(bb, &bb_r, dly_shift2);
|
|
enqueue(x, bb, &bb_w, dly_shift2 );
|
|
|
|
/* apply full delay to higher band */
|
|
x = FRACMUL(buf->p32[1][i], tcoef2);
|
|
temp1 += dequeue(b2, &b2_r, dly);
|
|
enqueue(x, b2, &b2_w, dly );
|
|
|
|
/* do the same direction trick again */
|
|
temp1 -= FRACMUL(temp1, hcoef);
|
|
|
|
x = FRACMUL(buf->p32[1][i], hcoef);
|
|
temp1 += dequeue(hh, &hh_r, dly_shift1);
|
|
enqueue(x, hh, &hh_w, dly_shift1 );
|
|
/*balance*/
|
|
if (surround_balance > 0 && !surround_side_only)
|
|
{
|
|
temp0 -= temp0/200 * surround_balance;
|
|
temp1 += temp1/200 * surround_balance;
|
|
}
|
|
else if (surround_balance > 0)
|
|
{
|
|
temp0 += temp0/200 * surround_balance;
|
|
temp1 -= temp1/200 * surround_balance;
|
|
}
|
|
|
|
if (surround_side_only)
|
|
{
|
|
temp0 += mid;
|
|
temp1 += mid;
|
|
}
|
|
|
|
if (surround_mix == 100)
|
|
{
|
|
buf->p32[0][i] = temp0;
|
|
buf->p32[1][i] = temp1;
|
|
}
|
|
else
|
|
{
|
|
/*dry wet mix*/
|
|
buf->p32[0][i] = buf->p32[0][i]/100 * (100-surround_mix) +
|
|
temp0/100 * surround_mix;
|
|
buf->p32[1][i] = buf->p32[1][i]/100 * (100-surround_mix) +
|
|
temp1/100 * surround_mix;
|
|
}
|
|
}
|
|
(void)this;
|
|
}
|
|
|
|
/* Handle format changes and verify the format compatibility */
|
|
static intptr_t surround_new_format(struct dsp_proc_entry *this,
|
|
struct dsp_config *dsp,
|
|
struct sample_format *format)
|
|
{
|
|
DSP_PRINT_FORMAT(DSP_PROC_SURROUND, *format);
|
|
|
|
/* Stereo mode only */
|
|
bool was_active = dsp_proc_active(dsp, DSP_PROC_SURROUND);
|
|
bool now_active = format->num_channels > 1;
|
|
dsp_proc_activate(dsp, DSP_PROC_SURROUND, now_active);
|
|
|
|
if (now_active)
|
|
{
|
|
if (!was_active)
|
|
dsp_surround_flush(); /* Going online */
|
|
|
|
return PROC_NEW_FORMAT_OK;
|
|
}
|
|
|
|
/* Can't do this. Sleep until next change. */
|
|
DEBUGF(" DSP_PROC_SURROUND- deactivated\n");
|
|
return PROC_NEW_FORMAT_DEACTIVATED;
|
|
|
|
(void)this;
|
|
}
|
|
|
|
/* DSP message hook */
|
|
static intptr_t surround_configure(struct dsp_proc_entry *this,
|
|
struct dsp_config *dsp,
|
|
unsigned int setting,
|
|
intptr_t value)
|
|
{
|
|
intptr_t retval = 0;
|
|
|
|
switch (setting)
|
|
{
|
|
case DSP_PROC_INIT:
|
|
/* Coming online; was disabled */
|
|
retval = surround_buffer_alloc();
|
|
if (retval < 0)
|
|
break;
|
|
|
|
this->process = surround_process;
|
|
|
|
dsp_surround_flush();
|
|
|
|
/* Wouldn't have been getting frequency updates */
|
|
surround_update_filter(dsp_get_output_frequency(dsp));
|
|
break;
|
|
|
|
case DSP_PROC_CLOSE:
|
|
/* Being disabled (called also if init fails) */
|
|
surround_buffer_free();
|
|
break;
|
|
|
|
case DSP_FLUSH:
|
|
/* Discontinuity; clear filters */
|
|
dsp_surround_flush();
|
|
break;
|
|
|
|
case DSP_SET_OUT_FREQUENCY:
|
|
/* New output frequency */
|
|
surround_update_filter(value);
|
|
break;
|
|
|
|
case DSP_PROC_NEW_FORMAT:
|
|
/* Source buffer format is changing (also sent when first enabled) */
|
|
retval = surround_new_format(this, dsp, (struct sample_format *)value);
|
|
break;
|
|
}
|
|
|
|
return retval;
|
|
}
|
|
|
|
/* Database entry */
|
|
DSP_PROC_DB_ENTRY(
|
|
SURROUND,
|
|
surround_configure);
|