168 lines
3.0 KiB
C
Executable File
168 lines
3.0 KiB
C
Executable File
#include <common/bk_include.h>
|
|
#include "bk_arm_arch.h"
|
|
|
|
#include "fft.h"
|
|
#include "bk_fft.h"
|
|
|
|
#include "bk_drv_model.h"
|
|
#include <driver/int.h>
|
|
|
|
#if CONFIG_FFT
|
|
static driver_fft_t driver_fft;
|
|
static const DD_OPERATIONS fft_op = {
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
fft_ctrl
|
|
};
|
|
|
|
static int fft_busy(void)
|
|
{
|
|
return driver_fft.busy_flag;
|
|
}
|
|
|
|
static void fft_enable(input_fft_t *fft_conf)
|
|
{
|
|
int i;
|
|
UINT32 value = 0;
|
|
|
|
REG_WRITE(FIR_CONF, 0);
|
|
REG_WRITE(FFT_CONF, 0);
|
|
|
|
if (fft_conf->mode == FFT_MODE_FFT)
|
|
value = FFT_CONF_INT_EN | FFT_CONF_ENABLE;
|
|
else if (fft_conf->mode == FFT_MODE_IFFT)
|
|
value = FFT_CONF_IFFT | FFT_CONF_INT_EN | FFT_CONF_ENABLE;
|
|
REG_WRITE(FFT_CONF, value);
|
|
|
|
driver_fft.in = fft_conf->inbuf;
|
|
driver_fft.out = fft_conf->outbuf;
|
|
driver_fft.size = fft_conf->size;
|
|
driver_fft.busy_flag = 1;
|
|
|
|
for (i = 0; i < fft_conf->size; i++) {
|
|
value = (UINT16)(fft_conf->inbuf[i]) << 16 | (UINT16)(fft_conf->outbuf[i]);
|
|
Fft_Write_Data(value);
|
|
}
|
|
|
|
Fft_Set_Start();
|
|
}
|
|
|
|
static void fir_single_enable(input_fir_t *fir_conf)
|
|
{
|
|
int i;
|
|
UINT32 value = 0;
|
|
|
|
if (fir_conf->fir_len > 64)
|
|
return;
|
|
|
|
REG_WRITE(FIR_CONF, 0);
|
|
REG_WRITE(FFT_CONF, 0);
|
|
|
|
value = (fir_conf->fir_len << FIR_CONF_LEN_POSI)
|
|
| (fir_conf->fir_cwidth << FIR_CONF_CWIDTH_POSI)
|
|
| (fir_conf->fir_dwidth << FIR_CONF_DWIDTH_POSI)
|
|
| FIR_CONF_INT_EN
|
|
| FIR_CONF_ENABLE;
|
|
REG_WRITE(FIR_CONF, value);
|
|
|
|
driver_fft.mac_out = fir_conf->mac;
|
|
driver_fft.busy_flag = 1;
|
|
|
|
for (i = 0; i < fir_conf->fir_len + 1; i++) {
|
|
Fft_Write_Coef(fir_conf->coef[i]);
|
|
Fft_Write_Data(fir_conf->input[i]);
|
|
}
|
|
|
|
Fft_Set_Start();
|
|
|
|
return;
|
|
}
|
|
|
|
static __inline INT16 f_sat(int din)
|
|
{
|
|
if (din > 32767)
|
|
return 32767;
|
|
if (din < -32767)
|
|
return -32767;
|
|
else
|
|
return (din);
|
|
}
|
|
|
|
void fft_init(void)
|
|
{
|
|
bk_int_isr_register(INT_SRC_FFT, fft_isr, NULL);
|
|
|
|
sddev_register_dev(DD_DEV_TYPE_FFT, (DD_OPERATIONS *)&fft_op);
|
|
}
|
|
|
|
void fft_exit(void)
|
|
{
|
|
sddev_unregister_dev(DD_DEV_TYPE_FFT);
|
|
}
|
|
|
|
UINT32 fft_ctrl(UINT32 cmd, void *param)
|
|
{
|
|
int ret = FFT_SUCCESS;
|
|
|
|
switch (cmd) {
|
|
case CMD_FFT_BUSY:
|
|
*((int *)param) = fft_busy();
|
|
break;
|
|
case CMD_FFT_ENABLE:
|
|
fft_enable((input_fft_t *)param);
|
|
break;
|
|
case CMD_FIR_SIGNLE_ENABLE:
|
|
fir_single_enable((input_fir_t *)param);
|
|
break;
|
|
default:
|
|
ret = FFT_FAILURE;
|
|
break;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
void fft_isr(void)
|
|
{
|
|
UINT32 int_status;
|
|
UINT8 FFT_int, FIR_int;
|
|
|
|
int_status = REG_READ(FFT_STATUS);
|
|
FFT_int = int_status & FFT_STATUS_DONE;
|
|
FIR_int = int_status & FIR_STATUS_DONE;
|
|
|
|
if (FFT_int) {
|
|
int i;
|
|
int bit_ext;
|
|
INT32 temp, temp_out;
|
|
|
|
bit_ext = (int_status & 0x00001fff) >> 7;
|
|
|
|
if (bit_ext & 0x20)
|
|
bit_ext = 64 - bit_ext;
|
|
else
|
|
bit_ext = 0;
|
|
|
|
for (i = 0; i < driver_fft.size; i++) {
|
|
temp = Fft_Read_Data();
|
|
temp_out = ((temp << 16) >> 16) << bit_ext ;
|
|
driver_fft.out[i] = f_sat(temp_out);
|
|
temp_out = (temp >> 16) << bit_ext;
|
|
driver_fft.in[i] = f_sat(temp_out);
|
|
}
|
|
|
|
driver_fft.busy_flag = 0;
|
|
REG_WRITE(FFT_CONF, 0);
|
|
}
|
|
|
|
if (FIR_int) {
|
|
*driver_fft.mac_out = REG_READ(FFT_MAC_LOW);
|
|
|
|
driver_fft.busy_flag = 0;
|
|
REG_WRITE(FIR_CONF, 0);
|
|
}
|
|
}
|
|
#endif
|