kunlun/sp/topo_cpu/hw_topo/wq_vtb_topo_coprocess.c

/****************************************************************************

Copyright(c) 2019 by Aerospace C.Power (Chongqing) Microelectronics. ALL RIGHTS RESERVED.

This Information is proprietary to Aerospace C.Power (Chongqing) Microelectronics and MAY NOT
be copied by any method or incorporated into another program without
the express written consent of Aerospace C.Power. This Information or any portion
thereof remains the property of Aerospace C.Power. The Information contained herein
is believed to be accurate and Aerospace C.Power assumes no responsibility or
liability for its use in any way and conveys no license or title under
any patent or copyright and makes no representation or warranty that this
Information is free from patent or copyright infringement.

****************************************************************************/
/* os shim includes */
#include "os_types.h"

#include "iot_config.h"
#include "iot_utils_api.h"

#include "wq_vtb_topo_intelnal.h"
#include "wq_vtb_topo_rec.h"
#include "iot_system.h"

extern cpu_state g_cpu1_state;

wq_vtb_topo_coprocess_op_t g_wq_vtb_topo_op = {0};

wq_vtb_decimation_filter_t g_decm_filter[WQ_VTB_TOPO_PHASE_CNT] = {0};
/* points to the cos table for spectrum shifting */
int32_t g_cos_tab[WQ_VTB_LOAD_SIN_TAB_MAX];
/* pointer to the sin table for spectrum shifting */
int32_t g_sin_tab[WQ_VTB_LOAD_SIN_TAB_MAX];

static wq_complex_t g_load_filter_buf[WQ_VTB_LOAD_RAW_DATA_NUM_MAX_INPUT + \
        (WQ_VTB_LOAD_RAW_DATA_NUM_MAX_INPUT / \
        WQ_VTB_LOAD_DECM_FILTER_DECM_RATION_MIN + 1) * 2];

static void wq_vtb_spectrum_shift_without_round(wq_vtb_decimation_filter_t *h,
    int32_t *raw, uint32_t len, wq_complex_t *out)
{
    uint32_t i;
    for (i = 0; i < len; i++) {
        out[i].real = (raw[i] * h->cos_tab[h->idx]) >> 15;
        out[i].imag = (raw[i] * h->sin_tab[h->idx]) >> 15;
        h->idx++;
        if( h->idx >= h->sin_tab_size) {
            h->idx = 0;
        }
    }
}

static void wq_vtb_spectrum_shift_with_round(wq_vtb_decimation_filter_t *h,
    int32_t *raw, uint32_t len, wq_complex_t *out)
{
    uint32_t i;
    for (i = 0; i < len; i++) {
        out[i].real = wq_vtb_integer_shift_round(
            raw[i] * h->cos_tab[h->idx], 15);
        out[i].imag = wq_vtb_integer_shift_round(
            raw[i] * h->sin_tab[h->idx], 15);
        h->idx++;
        if(h->idx >= h->sin_tab_size) {
            h->idx = 0;
        }
    }
}

static void wq_vtb_spectrum_shift(wq_vtb_decimation_filter_t *h, int32_t *raw,
    uint32_t len, wq_complex_t *out)
{
    if (h->is_fast) {
        wq_vtb_spectrum_shift_without_round(h, raw, len, out);
    } else {
        wq_vtb_spectrum_shift_with_round(h, raw, len, out);
    }
}

static void wq_filter(wq_filter_t *filter, wq_complex_t *in, wq_complex_t *out)
{
    uint8_t i, j;
    filter->buf[filter->idx].real  = in->real;
    filter->buf[filter->idx].imag  = in->imag;
    j = filter->idx;
    filter->idx++;
    if (filter->idx >= filter->order) {
        filter->idx = 0;
    }
    for (i = 0; i < filter->order; i++) {
        out->real += filter->h[i] * filter->buf[j].real;
        out->imag += filter->h[i] * filter->buf[j].imag;
        if (j != 0) {
            j--;
        } else {
            j = filter->order - 1;
        }
    }
}

static uint16_t wq_vtb_decimation_filter(wq_vtb_decimation_filter_t *h,
    wq_complex_t *in, uint32_t len, wq_complex_t *out)
{
    uint32_t i, j;

    len /= h->ratio;
    for (i = 0; i < len; i++) {
        out[i].real = 0;
        out[i].imag = 0;
        for (j = 0; j < h->ratio; j++) {
            wq_filter(&h->filter[j], &in[(h->ratio - 1) - j], &out[i]);
        }
        /* right shift some bits, avoid overflow during subsequent
         * calculation.
         */
        out[i].real >>= h->scale;
        out[i].imag >>= h->scale;
        in += h->ratio;
    }
    return (uint16_t)len;
}

void wq_vtb_topo_coprocess_init(void)
{
    g_sin_tab[0] = 0;
    g_cos_tab[0] = 0;
    g_wq_vtb_topo_op.cpu1_flag = g_cpu1_state.flags;
}

void wq_vtb_topo_coprocess(void)
{
    wq_complex_t * buf;
    uint32_t len, temp;
    int32_t *raw;
    wq_vtb_decimation_filter_t *decim_filter;
    if (g_wq_vtb_topo_op.cmd == WQ_VTB_LOAD_COPROCESS_CMD_T_CALC) {
        decim_filter = &g_decm_filter[g_wq_vtb_topo_op.phase];
        raw = (int32_t *)g_wq_vtb_topo_op.data1;
        len = g_wq_vtb_topo_op.data2;
        buf = g_load_filter_buf;
        wq_vtb_spectrum_shift(decim_filter, raw, len, buf);
        temp = len;
        len = wq_vtb_decimation_filter(decim_filter, buf, len, buf + len);
        buf += temp;
        g_wq_vtb_topo_op.data1 = (uint32_t)buf;
        g_wq_vtb_topo_op.data2 = len;
        g_wq_vtb_topo_op.cmd = 0;
        asm volatile("fence.i");
    }
}