rockbox/lib/rbcodec/dsp/pbe.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 "config.h"
#include "fixedpoint.h"
#include "fracmul.h"
#include "settings.h"
#include "dsp_proc_entry.h"
#include "dsp_misc.h"
#include "dsp_filter.h"
#include "core_alloc.h"

/* Perceptual bass enhancement */

#define B3_DLY 72    /* ~800 uS */
#define B2_DLY 180   /* ~2000 uS*/
#define B0_DLY 276   /* ~3050 uS */

#define B3_SIZE (B3_DLY+1)
#define B2_SIZE (B2_DLY+1)
#define B0_SIZE (B0_DLY+1)

static int pbe_strength = 100;
static int pbe_precut = 0;
static int32_t tcoef1, tcoef2, tcoef3;
static int32_t *b0[2], *b2[2], *b3[2];
static int b0_r[2],b2_r[2],b3_r[2],b0_w[2],b2_w[2],b3_w[2];
int32_t temp_buffer;
static struct dsp_filter pbe_filter[5];
static int handle = -1;

static void pbe_buffer_alloc(void)
{
    if (handle > 0)
        return; /* already-allocated */

    unsigned int total_len = (B0_SIZE + B2_SIZE + B3_SIZE) * 2;
    handle = core_alloc("dsp_pbe_buffer",sizeof(int32_t) * total_len);

    if (handle < 0)
    {
        pbe_strength = 0;
        return;
    }
    memset(core_get_data(handle),0,sizeof(int32_t) * total_len);
}

static void pbe_buffer_get_data(void)
{
    if (handle < 0)
        return;
    b0[0] = core_get_data(handle);
    b0[1] = b0[0] + B0_SIZE;
    b2[0] = b0[1] + B0_SIZE;
    b2[1] = b2[0] + B2_SIZE;
    b3[0] = b2[1] + B2_SIZE;
    b3[1] = b3[0] + B3_SIZE;
}

static void dsp_pbe_flush(void)
{
    if (pbe_strength == 0)
        return; /* Not currently enabled */

    pbe_buffer_get_data();

    memset(b0[0], 0, B0_DLY * sizeof(int32_t));
    memset(b0[1], 0, B0_DLY * sizeof(int32_t));
    memset(b2[0], 0, B2_DLY * sizeof(int32_t));
    memset(b2[1], 0, B2_DLY * sizeof(int32_t));
    memset(b3[0], 0, B3_DLY * sizeof(int32_t));
    memset(b3[1], 0, B3_DLY * sizeof(int32_t));
    b0_r[0] = 0; b0_w[0] = B0_DLY;
    b0_r[1] = 0; b0_w[1] = B0_DLY;
    b2_r[0] = 0; b2_w[0] = B2_DLY;
    b2_r[1] = 0; b2_w[1] = B2_DLY;
    b3_r[0] = 0; b3_w[0] = B3_DLY;
    b3_r[1] = 0; b3_w[1] = B3_DLY;

    for (int i = 0; i < 5; i++)
        filter_flush(&pbe_filter[i]);
}

static void pbe_update_filter(unsigned int fout)
{
    tcoef1 = fp_div(160, fout, 31);
    tcoef2 = fp_div(500, fout, 31);
    tcoef3 = fp_div(1150, fout, 31);
    /* Biophonic EQ */
    filter_bishelf_coefs(fp_div(20, fout, 32),
                         fp_div(16000, fout, 32),
                         0, 53, -5 + pbe_precut,
                         &pbe_filter[0]);
    filter_pk_coefs(fp_div(64, fout, 32), 28, 53,
                     &pbe_filter[1]);
    filter_pk_coefs(fp_div(2000, fout, 32), 28, 58,
                     &pbe_filter[2]);
    filter_pk_coefs(fp_div(7500, fout, 32), 43, -82,
                     &pbe_filter[3]);
    filter_pk_coefs(fp_div(10000, fout, 32), 43, -29,
                     &pbe_filter[4]);

}

void dsp_pbe_precut(int var)
{
    if (var == pbe_precut)
        return; /* No change */

    pbe_precut = var;

    if (pbe_strength == 0)
        return; /* Not currently enabled */

    /* Push more DSP_PROC_INIT messages to force filter updates
       (with value = 1) */
    struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
    dsp_proc_enable(dsp, DSP_PROC_PBE, true);
}


void dsp_pbe_enable(int var)
{
    if (var == pbe_strength)
        return; /* No change */
    bool was_enabled = pbe_strength > 0;
    pbe_strength = var;

    bool now_enabled = var > 0;

    if (now_enabled == was_enabled)
        return; /* No change in enabled status */

    if (now_enabled == false && handle > 0)
    {
        core_free(handle);
        handle = -1;
    }

    struct dsp_config *dsp = dsp_get_config(CODEC_IDX_AUDIO);
    dsp_proc_enable(dsp, DSP_PROC_PBE, now_enabled);
}

static void pbe_process(struct dsp_proc_entry *this,
                        struct dsp_buffer **buf_p)
{
    struct dsp_buffer *buf = *buf_p;
    int count = buf->remcount;
    int num_channels = buf->format.num_channels;
    int b2_level = (B2_DLY * pbe_strength) / 100;
    int b0_level = (B0_DLY * pbe_strength) / 100;
    int32_t x;

    pbe_buffer_get_data();

    for(int ch = 0; ch < num_channels; ch++)
    {
        for (int i = 0; i < count; i++)
        {
            /* 160hz - 500hz no delay */
            temp_buffer = FRACMUL(buf->p32[ch][i], tcoef1) -
                          FRACMUL(buf->p32[ch][i], tcoef2);

            /* delay below 160hz*/
            x = buf->p32[ch][i] -
                FRACMUL(buf->p32[ch][i], tcoef1);
            temp_buffer += dequeue(b0[ch], &b0_r[ch], b0_level);
            enqueue(x, b0[ch], &b0_w[ch], b0_level );

            /* delay 500-1150hz */
            x = FRACMUL(buf->p32[ch][i], tcoef2) -
                FRACMUL(buf->p32[ch][i], tcoef3);
            temp_buffer += dequeue(b2[ch], &b2_r[ch], b2_level);
            enqueue(x, b2[ch], &b2_w[ch], b2_level );

            /* delay anything beyond 1150hz */
            x = FRACMUL(buf->p32[ch][i], tcoef3);
            temp_buffer += dequeue(b3[ch], &b3_r[ch], B3_DLY);
            enqueue(x, b3[ch], &b3_w[ch], B3_DLY );

            buf->p32[ch][i] = temp_buffer;
        }
    }

    /* apply Biophonic EQ   */
    for (int i = 0; i < 5; i++)
        filter_process(&pbe_filter[i], buf->p32, buf->remcount,
                       buf->format.num_channels);

    (void)this;
}

/* DSP message hook */
static intptr_t pbe_configure(struct dsp_proc_entry *this,
                                     struct dsp_config *dsp,
                                     unsigned int setting,
                                     intptr_t value)
{
    /* This only attaches to the audio (codec) DSP */

    switch (setting)
    {
    case DSP_PROC_INIT:
        if (value == 0)
        {
            /* Coming online; was disabled */
            this->process = pbe_process;
            pbe_buffer_alloc();
            dsp_pbe_flush();
            dsp_proc_activate(dsp, DSP_PROC_PBE, true);
        }
        /* else additional forced messages */

        pbe_update_filter(dsp_get_output_frequency(dsp));
        break;

    case DSP_FLUSH:
        /* Discontinuity; clear filters */
        dsp_pbe_flush();
        break;

    case DSP_SET_OUT_FREQUENCY:
        /* New output frequency */
        pbe_update_filter(value);
        break;
    }

    return 1;
}

/* Database entry */
DSP_PROC_DB_ENTRY(
    PBE,
    pbe_configure);