985 lines
30 KiB
C
985 lines
30 KiB
C
#ifdef RTOS
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
|
|
#include "hal_dma.h"
|
|
#include "hal_timer.h"
|
|
#include "hal_trace.h"
|
|
#include "heap_api.h"
|
|
|
|
#include "string.h"
|
|
#include "bt_drv.h"
|
|
#include "cmsis_os.h"
|
|
#include "hal_btdump.h"
|
|
#include "iqcorrect.h"
|
|
#include "nvrecord.h"
|
|
#include "nvrecord_env.h"
|
|
#include "nvrecord_dev.h"
|
|
#ifdef TX_IQ_CAL
|
|
#define LOG_MODULE HAL_TRACE_MODULE_APP
|
|
|
|
#define NTbl (1024)
|
|
#define fftSize 1024
|
|
|
|
#define bitshift 10
|
|
#define pi (3.1415926535898)
|
|
#define min(a,b) (((a) < (b)) ? (a) : (b))
|
|
|
|
#define READ_REG(b,a) *(volatile uint32_t *)(b+a)
|
|
#define WRITE_REG(v,b,a) *(volatile uint32_t *)(b+a) = v
|
|
|
|
#define BUF_SIZE (1024)
|
|
|
|
#define MAX_COUNT 5
|
|
volatile int iqimb_dma_status = 0;
|
|
//short M0data[BUF_SIZE];
|
|
#define MED_MEM_POOL_SIZE (20*1024)
|
|
static uint8_t *g_medMemPool = NULL;
|
|
|
|
extern void *rt_malloc(unsigned int size);
|
|
extern void rt_free(void *rmem);
|
|
|
|
typedef struct ComplexInt_
|
|
{
|
|
int re;
|
|
int im;
|
|
} ComplexInt;
|
|
typedef struct Complexflt_
|
|
{
|
|
float re;
|
|
float im;
|
|
} ComplexFlt;
|
|
typedef struct ComplexShort_
|
|
{
|
|
short re;
|
|
short im;
|
|
} ComplexShort;
|
|
|
|
#ifdef DCCalib
|
|
static void Tblgen(ComplexShort*w0, ComplexShort*w1, ComplexShort*w2,int len)
|
|
{
|
|
for (int i=0; i<len; i++)
|
|
{
|
|
|
|
w0[i].re = (short)(cos(2*pi*i/(2048.0/32))*32767);
|
|
w0[i].im = (short)(sin(2*pi*i/(2048.0/32))*32767);
|
|
w1[i].re = (short)(cos(2*pi*i/(2048.0/31))*32767);
|
|
w1[i].im = (short)(sin(2*pi*i/(2048.0/31))*32767);
|
|
w2[i].re = (short)(cos(2*pi*i/(2048.0/33))*32767);
|
|
w2[i].im = (short)(sin(2*pi*i/(2048.0/33))*32767);
|
|
}
|
|
}
|
|
#endif
|
|
static void Tblgen_iq(ComplexShort*w0, ComplexShort*w1, ComplexShort*w2,int len)
|
|
{
|
|
for (int i=0; i<len; i++)
|
|
{
|
|
|
|
w0[i].re = (short)(cos(2*pi*i/(2048.0/64))*32767);
|
|
w0[i].im = (short)(sin(2*pi*i/(2048.0/64))*32767);
|
|
w1[i].re = (short)(cos(2*pi*i/(2048.0/63))*32767);
|
|
w1[i].im = (short)(sin(2*pi*i/(2048.0/63))*32767);
|
|
w2[i].re = (short)(cos(2*pi*i/(2048.0/65))*32767);
|
|
w2[i].im = (short)(sin(2*pi*i/(2048.0/65))*32767);
|
|
}
|
|
}
|
|
/*
|
|
typedef struct IQMismatchPreprocessState_
|
|
{
|
|
short *M0data;
|
|
ComplexShort *Table0;
|
|
ComplexShort *Table1;
|
|
ComplexShort *Table2;
|
|
} IQMismatchPreprocessState;
|
|
|
|
|
|
IQMismatchPreprocessState st_g;
|
|
short st_M0data[fftSize];
|
|
short st_Table0[2*NTbl];
|
|
short st_Table1[2*NTbl];
|
|
short st_Table2[2*NTbl];
|
|
|
|
IQMismatchPreprocessState *st;
|
|
|
|
|
|
IQMismatchPreprocessState *IQMismatchPreprocessState_init(int Cohcnt)
|
|
{
|
|
st_g.M0data = st_M0data;
|
|
st_g.Table0= st_Table0;
|
|
st_g.Table1= st_Table1;
|
|
st_g.Table2= st_Table2;
|
|
return &st_g;
|
|
}
|
|
*/
|
|
|
|
typedef struct IQMismatchPreprocessState_
|
|
{
|
|
short *M0data;
|
|
ComplexShort *Table0;
|
|
ComplexShort *Table1;
|
|
ComplexShort *Table2;
|
|
} IQMismatchPreprocessState;
|
|
|
|
IQMismatchPreprocessState *IQMismatchPreprocessState_init(int fft_size)
|
|
{
|
|
BT_DRV_TRACE(0,"malloc ini");
|
|
IQMismatchPreprocessState *st = (IQMismatchPreprocessState *)med_calloc(1, sizeof(IQMismatchPreprocessState));
|
|
st->M0data = (short*)med_calloc(fft_size, sizeof(short));
|
|
st->Table0 = (ComplexShort*)med_calloc(fft_size, sizeof(ComplexShort));
|
|
st->Table1 = (ComplexShort*)med_calloc(fft_size, sizeof(ComplexShort));
|
|
st->Table2 = (ComplexShort*)med_calloc(fft_size, sizeof(ComplexShort));
|
|
BT_DRV_TRACE(5,"st:%p, st->M0data:%p, st->Table0:%p, st->Table1:%p, st->Table2:%p", st, st->M0data, st->Table0, st->Table1, st->Table2);
|
|
BT_DRV_TRACE(0,"malloc ok");
|
|
return st;
|
|
}
|
|
|
|
int32_t IQMismatchPreprocessState_destroy(IQMismatchPreprocessState *st)
|
|
{
|
|
BT_DRV_TRACE(5,"st:%p, st->M0data:%p, st->Table0:%p, st->Table1:%p, st->Table2:%p", st, st->M0data, st->Table0, st->Table1, st->Table2);
|
|
med_free(st->M0data);
|
|
med_free(st->Table0);
|
|
med_free(st->Table1);
|
|
med_free(st->Table2);
|
|
med_free(st);
|
|
return 0;
|
|
}
|
|
|
|
int IQMismatchParameterCalc_ex(short *M0data,
|
|
short Cohcnt, ComplexShort *Table0, ComplexShort *Table1, ComplexShort *Table2,int fftsize)
|
|
{
|
|
int i,j,k;
|
|
float M0 = 0;
|
|
//ComplexInt tmp0;
|
|
ComplexFlt tmp0;
|
|
ComplexFlt tmp1;
|
|
ComplexFlt tmp2;
|
|
for (j = 0; j < Cohcnt; j++)
|
|
{
|
|
tmp0.re = 0.0f;
|
|
tmp0.im = 0.0f;
|
|
tmp1.re = 0.0f;
|
|
tmp1.im = 0.0f;
|
|
tmp2.re = 0.0f;
|
|
tmp2.im = 0.0f;
|
|
k = 0;
|
|
for (i=0; i<fftsize; i++)
|
|
{
|
|
if(k>=fftsize)
|
|
k = 0;
|
|
tmp0.re = tmp0.re + (((int)M0data[i] *(int)Table0[k].re) >> 15) ;
|
|
tmp0.im = tmp0.im + (((int)M0data[i] * (int)Table0[k].im) >> 15) ;
|
|
tmp1.re = tmp1.re + (((int)M0data[i] *(int)Table1[k].re) >> 15) ;
|
|
tmp1.im = tmp1.im + (((int)M0data[i] * (int)Table1[k].im) >> 15) ;
|
|
tmp2.re = tmp2.re + (((int)M0data[i] *(int)Table2[k].re) >> 15) ;
|
|
tmp2.im = tmp2.im + (((int)M0data[i] * (int)Table2[k].im) >> 15) ;
|
|
k = k + 1;
|
|
}
|
|
tmp0.re = tmp0.re/ fftsize;// >> bitshift;
|
|
tmp0.im = tmp0.im/ fftsize;// >>bitshift;
|
|
tmp1.re = tmp1.re/ fftsize;// >> bitshift;
|
|
tmp1.im = tmp1.im/ fftsize;// >>bitshift;
|
|
tmp2.re = tmp2.re/ fftsize;// >> bitshift;
|
|
tmp2.im = tmp2.im/ fftsize;// >>bitshift;
|
|
M0 = M0 + tmp0.re*tmp0.re + tmp0.im*tmp0.im;
|
|
M0 = M0 + tmp1.re*tmp1.re + tmp1.im*tmp1.im;
|
|
M0 = M0 + tmp2.re*tmp2.re + tmp2.im*tmp2.im;
|
|
}
|
|
return (int)(M0/Cohcnt);
|
|
}
|
|
|
|
|
|
void caculate_energy_main_test(IQMismatchPreprocessState *st,int* Energy,int* Energy1,int fftsize)
|
|
{
|
|
short Cohcnt = 1;
|
|
*Energy1 = IQMismatchParameterCalc_ex(st->M0data, Cohcnt, st->Table0, st->Table1, st->Table2,fftsize);
|
|
}
|
|
|
|
static struct HAL_DMA_DESC_T iqimb_dma_desc[1];
|
|
|
|
|
|
static void iqimb_dma_dout_handler(uint32_t remains, uint32_t error, struct HAL_DMA_DESC_T *lli)
|
|
{
|
|
hal_audma_free_chan(0);
|
|
hal_btdump_disable();
|
|
iqimb_dma_status = 0;
|
|
|
|
return;
|
|
}
|
|
|
|
int bt_iqimb_dma_enable ( short * dma_dst_data, uint16_t size)
|
|
{
|
|
struct HAL_DMA_CH_CFG_T iqimb_dma_cfg;
|
|
// BT_DRV_TRACE(0,"bt_iqimb_dma_enable :");
|
|
|
|
iqimb_dma_status = 1;
|
|
|
|
memset(&iqimb_dma_cfg, 0, sizeof(iqimb_dma_cfg));
|
|
|
|
//iqimb_dma_cfg.ch = hal_audma_get_chan(HAL_AUDMA_DSD_RX,HAL_DMA_HIGH_PRIO);
|
|
|
|
iqimb_dma_cfg.ch = hal_audma_get_chan(HAL_AUDMA_BTDUMP,HAL_DMA_HIGH_PRIO);
|
|
iqimb_dma_cfg.dst_bsize = HAL_DMA_BSIZE_16;
|
|
iqimb_dma_cfg.dst_periph = 0; //useless
|
|
iqimb_dma_cfg.dst_width = HAL_DMA_WIDTH_WORD;
|
|
iqimb_dma_cfg.handler = (HAL_DMA_IRQ_HANDLER_T)iqimb_dma_dout_handler;
|
|
iqimb_dma_cfg.src = 0; // useless
|
|
iqimb_dma_cfg.src_bsize = HAL_DMA_BSIZE_4;
|
|
|
|
//iqimb_dma_cfg.src_periph = HAL_AUDMA_DSD_RX;
|
|
iqimb_dma_cfg.src_periph = HAL_AUDMA_BTDUMP;
|
|
iqimb_dma_cfg.src_tsize = size;//1600; //1600*2/26=123us
|
|
iqimb_dma_cfg.src_width = HAL_DMA_WIDTH_WORD;
|
|
//iqimb_dma_cfg.src_width = HAL_DMA_WIDTH_HALFWORD;
|
|
iqimb_dma_cfg.try_burst = 1;
|
|
iqimb_dma_cfg.type = HAL_DMA_FLOW_P2M_DMA;
|
|
iqimb_dma_cfg.dst = (uint32_t)(dma_dst_data);
|
|
|
|
hal_audma_init_desc(&iqimb_dma_desc[0], &iqimb_dma_cfg, 0, 1);
|
|
|
|
hal_audma_sg_start(&iqimb_dma_desc[0], &iqimb_dma_cfg);
|
|
|
|
//configed after mismatch parameter done, or apb clock muxed.
|
|
|
|
//wait
|
|
for(volatile int i=0; i<5000; i++);
|
|
hal_btdump_enable();
|
|
|
|
return 1;
|
|
|
|
}
|
|
|
|
int bt_iqimb_ini ()
|
|
{
|
|
uint32_t val;
|
|
|
|
BT_DRV_TRACE(0,"bt_iqimb_ini :");
|
|
//config gpadc 26m
|
|
val = READ_REG(0xd0350208,0x0);
|
|
val &= 0xffefffff; //bit20
|
|
val |= (1<<18); //reg_gclk_en(BTM_CLK_26RXNFMI)
|
|
val &= 0xffbfffff; //bit22
|
|
val |= (1<<20); //reg_gclk_en(BTM_CLK_26RXDUMP)
|
|
val &= 0xfdffffff; //bit25
|
|
val |= (1<<25); //reg_gclk_en(BTM_CLK_26RXADC)
|
|
WRITE_REG(val,0xd0350208,0x0);
|
|
BT_DRV_TRACE(1,"0xd0350208 : 0x%08x",val);
|
|
|
|
val = READ_REG(0xd0350228,0x0);
|
|
val &= 0xffefffff; //bit20
|
|
val |= (1<<18); //reg_gclk_mode(BTM_CLK_26RXNFMI)
|
|
val &= 0xffbfffff; //bit22
|
|
val |= (1<<20); //reg_gclk_mode(BTM_CLK_26RXDUMP)
|
|
val &= 0xfdffffff; //bit25
|
|
val |= (1<<25); //reg_gclk_mode(BTM_CLK_26RXADC)
|
|
WRITE_REG(val,0xd0350228,0x0);
|
|
BT_DRV_TRACE(1,"0xd0350228 : 0x%08x",val);
|
|
|
|
//for rx mux to gpadc
|
|
val = READ_REG(0xd0350218,0x0);
|
|
val &= 0xfffffff0; //bit3:0 1110 reg_diagcntl(3 downto 0)
|
|
val |= (0xe<<0);
|
|
val &= 0xfffffbff; //bit10 1 reg_diagcntl(10)
|
|
val |= (0x1<<10);
|
|
WRITE_REG(val,0xd0350218,0x0);
|
|
|
|
BT_DRV_TRACE(1,"0xd0350218 : 0x%08x",val);
|
|
|
|
|
|
val = READ_REG(0xd0330024,0x0);
|
|
val &= 0xfffffff7; //bit3:0 1110 reg_diagcntl(3 downto 0)
|
|
val |= (0x1<<15);
|
|
WRITE_REG(val,0xd0330024,0x0);
|
|
BT_DRV_TRACE(1,"0xd0330024 : 0x%08x",val);
|
|
|
|
|
|
val = READ_REG(0xd0330034,0x0);
|
|
val &= 0x03ffffff; //bit3:0 1110 reg_diagcntl(3 downto 0)
|
|
val |= (0x1b<<26);
|
|
WRITE_REG(val,0xd0330034,0x0);
|
|
|
|
BT_DRV_TRACE(1,"0xd0330034 : 0x%08x",val);
|
|
|
|
WRITE_REG(0x002eb948,0xd0350364,0x0);
|
|
|
|
|
|
val = READ_REG(0xd0330038,0x0);
|
|
|
|
val |= (0x1<<24);
|
|
WRITE_REG(val,0xd0330038,0x0);
|
|
|
|
|
|
WRITE_REG(0,0xc000033c,0x0);
|
|
return 1;
|
|
}
|
|
void check_mem_data(void* data, int len)
|
|
{
|
|
short* share_mem = (short*)data;
|
|
BT_DRV_TRACE(3,"check_mem_data :share_mem= %p, 0x%x, 0x%x",share_mem,share_mem[0],share_mem[1]);
|
|
|
|
int16_t i =0;
|
|
int16_t m =0;
|
|
int16_t remain = len;
|
|
for(m=0; m<32; m++)
|
|
{
|
|
for(i=0; i<32; i++)
|
|
{
|
|
|
|
if (remain >16)
|
|
{
|
|
DUMP16("%04x ",share_mem,16);
|
|
share_mem +=16;
|
|
remain -= 16;
|
|
}
|
|
else
|
|
{
|
|
DUMP16("%04x ",share_mem,remain);
|
|
|
|
remain =0;
|
|
return;
|
|
}
|
|
|
|
}
|
|
// BT_DRV_TRACE(0,"\n");
|
|
//BT_DRV_TRACE(1,"addr :0x%08x\n",share_mem);
|
|
hal_sys_timer_delay(MS_TO_TICKS(200));
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
int bt_Txdc_cal_set(int ch_num, int dc_add)
|
|
{
|
|
uint32_t val;
|
|
uint32_t tmp = (uint32_t)dc_add;
|
|
//sel apb clock
|
|
val = READ_REG(0xd0350348,0x0);
|
|
if(ch_num==0)
|
|
{
|
|
val &= 0xfffffc00; //bit31 1 int_en_mismatch
|
|
val |= tmp & 0x3ff;
|
|
}
|
|
else
|
|
{
|
|
// BT_DRV_TRACE(1,"bt_Txdc_cal_set, dcadd : 0x%08x",tmp);
|
|
val &= 0xfc00ffff; //bit31 1 int_en_mismatch
|
|
val |= (tmp & 0x3ff) <<16;
|
|
}
|
|
WRITE_REG(val,0xd0350348,0x0);
|
|
|
|
|
|
// BT_DRV_TRACE(1,"bt_Txdc_cal_set, 0xd0350348 : 0x%08x",val);
|
|
|
|
return 1;
|
|
}
|
|
|
|
//g/p mismatch base addr 0xd0310000
|
|
//dc_i: 0xd0350348 9:0
|
|
//dc_q: 0xd0350348 25:16
|
|
int bt_iqimb_add_mismatch(int ch_num, int gain_mis, int phase_mis, int dc_i, int dc_q, uint32_t addr)
|
|
{
|
|
uint32_t val;
|
|
//sel apb clock
|
|
val = READ_REG(0xd0350348,0x0);
|
|
val &= 0x7fffffff; //bit31 1 int_en_mismatch
|
|
val |= (0x0<<31);
|
|
WRITE_REG(val,0xd0350348,0x0);
|
|
|
|
val = (phase_mis << 16) | (gain_mis & 0x0000ffff);
|
|
WRITE_REG(val,addr,ch_num*4);
|
|
//tval = READ_REG(addr,0x0);
|
|
//BT_DRV_TRACE(1,"bt_iqimb_add_mismatch, iq : 0x%08x",tval);
|
|
|
|
WRITE_REG(0x400000,0xd0350220,0);
|
|
WRITE_REG(0x400000,0xd0350224,0);
|
|
// bt_Txdc_cal_set(0,dc_i);
|
|
//bt_Txdc_cal_set(1,dc_q);
|
|
|
|
/*
|
|
val = READ_REG(0xd0350348,0x0);
|
|
val &= 0xfffffc00; //bit9:0
|
|
val |= dc_i;
|
|
val &= 0xfc00ffff; //bit25:16
|
|
val |= (dc_q<<16);
|
|
WRITE_REG(val,0xd0350348,0x0);
|
|
*/
|
|
//sel 26m clock
|
|
val = READ_REG(0xd0350348,0x0);
|
|
val &= 0x7fffffff; //bit31 1 int_en_mismatch
|
|
val |= (0x1<<31);
|
|
WRITE_REG(val,0xd0350348,0x0);
|
|
|
|
return 1;
|
|
}
|
|
|
|
void DC_correction(IQMismatchPreprocessState *st,int *dc_i_r,int *dc_q_r,int fftsize)
|
|
{
|
|
uint8_t k;
|
|
int Energy,Energy1,tmp;
|
|
int dc_iters = 2;
|
|
int dc_i;
|
|
int dc_q;
|
|
int dc_step = 4;
|
|
int dc_i_base;
|
|
int dc_q_base;
|
|
int P0,PIplus,PIneg,PQplus,PQneg,PIPQ;
|
|
int CI,CQ,tmp_D;
|
|
dc_i_base =0;
|
|
dc_q_base = 0;
|
|
for(k=0; k<dc_iters; k++)
|
|
{
|
|
dc_i = dc_i_base;
|
|
dc_q = dc_q_base;
|
|
bt_Txdc_cal_set(0,dc_i);
|
|
bt_Txdc_cal_set(1,dc_q);
|
|
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
P0 = Energy1;
|
|
dc_i = dc_i_base + dc_step;
|
|
dc_q = dc_q_base;
|
|
bt_Txdc_cal_set(0,dc_i);
|
|
bt_Txdc_cal_set(1,dc_q);
|
|
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
PIplus = Energy1;
|
|
dc_i = dc_i_base - dc_step;
|
|
dc_q = dc_q_base;
|
|
bt_Txdc_cal_set(0,dc_i);
|
|
bt_Txdc_cal_set(1,dc_q);
|
|
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
PIneg = Energy1;
|
|
dc_i = dc_i_base;
|
|
dc_q = dc_q_base + dc_step;
|
|
bt_Txdc_cal_set(0,dc_i);
|
|
bt_Txdc_cal_set(1,dc_q);
|
|
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
PQplus = Energy1;
|
|
dc_i = dc_i_base;
|
|
dc_q = dc_q_base - dc_step;
|
|
bt_Txdc_cal_set(0,dc_i);
|
|
bt_Txdc_cal_set(1,dc_q);
|
|
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
PQneg = Energy1;
|
|
dc_i = dc_i_base + dc_step;
|
|
dc_q = dc_q_base + dc_step;
|
|
bt_Txdc_cal_set(0,dc_i);
|
|
bt_Txdc_cal_set(1,dc_q);
|
|
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
PIPQ = Energy1;
|
|
tmp = 2*PIplus*PIplus + 2*PQplus*PQplus + 2*PIPQ*PIPQ;
|
|
tmp = tmp - 6*P0*P0;
|
|
tmp = tmp + 4*P0*(PIneg+PQneg+PIPQ);
|
|
tmp = tmp + 2*(PIplus*(PQplus - PQneg)-PIneg*(PQplus+PQneg));
|
|
tmp_D = tmp - 4*PIPQ*(PIplus+PQplus);
|
|
tmp = P0*(2*(PIplus - PIneg)+PQplus -PQneg);
|
|
tmp = tmp - PIPQ*(PQneg-PQplus)+PIneg*(PQplus+PQneg);
|
|
tmp = tmp + PQplus*(PQneg-PQplus-2*PIplus);
|
|
CI = -dc_step*tmp;
|
|
tmp = P0*(2*(PQplus - PQneg)+PIplus-PIneg);
|
|
tmp = tmp - PIPQ*(PIneg-PIplus)+PQneg*(PIplus+PIneg);
|
|
tmp = tmp + PIplus*(PIneg-PIplus-2*PQplus);
|
|
CQ = -dc_step*tmp;
|
|
dc_i_base = dc_i_base + CI/tmp_D;
|
|
dc_q_base = dc_q_base + CQ/tmp_D;
|
|
//dc_step = dc_step/2;
|
|
}
|
|
*dc_i_r = dc_i_base;
|
|
*dc_q_r = dc_q_base;
|
|
}
|
|
int IQ_GAIN_Mismatch_Correction(IQMismatchPreprocessState *st,int phase_mis_base,int fftsize,uint32_t addr)
|
|
{
|
|
uint8_t k = 0;
|
|
int phase_mis_tmp =0;
|
|
int gain_mis_tmp =0;
|
|
int tmp = 0;
|
|
int energy_ret0_last = 1048576;
|
|
int energy_ret0 = 0;
|
|
int energy_ret1 = 0;
|
|
int energy_ret2 = 0;
|
|
int Energy,Energy1 ;
|
|
int iters = 4;
|
|
int gainstep =8;
|
|
int gain_mis_base = 0;
|
|
|
|
phase_mis_tmp = phase_mis_base;
|
|
energy_ret0_last = 1048576;
|
|
for(k=0; k<iters+2; k++)
|
|
{
|
|
gain_mis_tmp = gain_mis_base;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret0 = Energy1;
|
|
if(energy_ret0<50)
|
|
break;
|
|
if(k>0)
|
|
{
|
|
if(energy_ret0_last < energy_ret0)
|
|
{
|
|
gain_mis_base = gain_mis_base + tmp;
|
|
gain_mis_tmp = gain_mis_base;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret0 = Energy1;
|
|
}
|
|
}
|
|
//BT_DRV_TRACE(1,"IQ gain correct energy_ret0 = %d",energy_ret0);
|
|
gainstep = 4;///////////////////
|
|
gain_mis_tmp = gain_mis_base + gainstep;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret2 = Energy1;
|
|
gain_mis_tmp = gain_mis_base - gainstep;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret1 = Energy1;
|
|
tmp = energy_ret2 -2*energy_ret0 + energy_ret1;
|
|
//BT_DRV_TRACE(1,"IQ gain correct energy_ret2 -2*energy_ret0 + energy_ret1 = %d",tmp);
|
|
if(tmp>0)
|
|
{
|
|
tmp = (energy_ret2-energy_ret1)*gainstep/tmp/2;
|
|
tmp = min(tmp,4*gainstep);
|
|
gainstep = gainstep/2;
|
|
}
|
|
else
|
|
{
|
|
tmp = 0;
|
|
iters = iters+1;
|
|
}
|
|
if(iters>8)
|
|
break;
|
|
if((gain_mis_base - tmp>60)||(gain_mis_base - tmp<-60))
|
|
tmp = 0;
|
|
//BT_DRV_TRACE(2,"IQ gain correct gain_mis = %d ,gain_adj = %d",gain_mis_base,tmp);
|
|
gain_mis_base = gain_mis_base - tmp;
|
|
energy_ret0_last = energy_ret0;
|
|
}
|
|
return gain_mis_base;
|
|
}
|
|
int IQ_Phase_Mismatch_Correction(IQMismatchPreprocessState *st,int gain_mis_base,int fftsize,uint32_t addr)
|
|
{
|
|
uint8_t k = 0;
|
|
int phase_mis_tmp =0;
|
|
int gain_mis_tmp =0;
|
|
int tmp = 0;
|
|
int energy_ret0_last = 1048576;
|
|
int energy_ret0 = 0;
|
|
int energy_ret1 = 0;
|
|
int energy_ret2 = 0;
|
|
int Energy,Energy1 ;
|
|
int iters = 4;
|
|
int phasestep =4;
|
|
int phase_mis_base = 0;
|
|
tmp = 0;
|
|
energy_ret0_last = 1048576;
|
|
gain_mis_tmp = gain_mis_base;
|
|
for(k=0; k<iters; k++)
|
|
{
|
|
phase_mis_tmp = phase_mis_base;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret0 = Energy1;
|
|
if(k>0)
|
|
{
|
|
if(energy_ret0_last < energy_ret0)
|
|
{
|
|
phase_mis_base = phase_mis_base + tmp;
|
|
phase_mis_tmp = phase_mis_base;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret0 = Energy1;
|
|
}
|
|
}
|
|
if(energy_ret0<50)
|
|
break;
|
|
// BT_DRV_TRACE(1,"IQ phase correct energy_ret0 = %d",energy_ret0);
|
|
phasestep = 4;//////////////////////////
|
|
phase_mis_tmp = phase_mis_base + phasestep;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret2 = Energy1;
|
|
// BT_DRV_TRACE(1,"IQ phase correct energy_ret2 = %d",energy_ret2);
|
|
phase_mis_tmp = phase_mis_base - phasestep;
|
|
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
energy_ret1 = Energy1;
|
|
//BT_DRV_TRACE(1,"IQ phase correct energy_ret1 = %d",energy_ret1);
|
|
tmp = energy_ret2 -2*energy_ret0 + energy_ret1;
|
|
// BT_DRV_TRACE(1,"IQ phase correct energy_ret2 -2*energy_ret0 + energy_ret1 = %d",tmp);
|
|
if(tmp>0)
|
|
{
|
|
tmp = (energy_ret2-energy_ret1)*phasestep/tmp/2;
|
|
tmp = min(tmp,4*phasestep);
|
|
phasestep = phasestep/2;
|
|
}
|
|
else
|
|
{
|
|
tmp = 0;
|
|
iters = iters + 1;
|
|
}
|
|
if(iters>8)
|
|
break;
|
|
if((phase_mis_base - tmp>60)||(phase_mis_base - tmp<-60))
|
|
tmp = 0;
|
|
// BT_DRV_TRACE(2,"IQ phase correct phase_mis = %d ,phase_adj = %d",phase_mis_base,tmp);
|
|
phase_mis_base = phase_mis_base - tmp;
|
|
energy_ret0_last = energy_ret0;
|
|
|
|
}
|
|
return phase_mis_base;
|
|
}
|
|
|
|
static BT_IQ_CALIBRATION_CONFIG_T config;
|
|
|
|
static void bt_update_local_iq_calibration_val(uint32_t freq_range, uint16_t gain_cal_val, uint16_t phase_cal_val)
|
|
{
|
|
config.validityMagicNum = BT_IQ_VALID_MAGIC_NUM;
|
|
config.gain_cal_val[freq_range] = gain_cal_val;
|
|
config.phase_cal_val[freq_range] = phase_cal_val;
|
|
}
|
|
|
|
// do QA calibration and get the calibration value
|
|
static POSSIBLY_UNUSED void btIqCalibration(void)
|
|
{
|
|
btdrv_tx_iq_cal();
|
|
BT_DRV_TRACE(0,"Acquired calibration value:");
|
|
for (uint32_t range = 0;range < BT_FREQENCY_RANGE_NUM;range++)
|
|
{
|
|
BT_DRV_TRACE(3,"%d: 0x%x - 0x%x", range,
|
|
config.gain_cal_val[range],
|
|
config.phase_cal_val[range]);
|
|
}
|
|
}
|
|
|
|
// get IQ calibration from NV
|
|
static bool bt_get_iq_calibration_val_from_nv(BT_IQ_CALIBRATION_CONFIG_T* pConfig)
|
|
{
|
|
if (nv_record_get_extension_entry_ptr() &&
|
|
(BT_IQ_VALID_MAGIC_NUM ==
|
|
nv_record_get_extension_entry_ptr()->btIqCalConfig.validityMagicNum))
|
|
{
|
|
*pConfig = nv_record_get_extension_entry_ptr()->btIqCalConfig;
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void bt_iq_calibration_setup(void)
|
|
{
|
|
if (bt_get_iq_calibration_val_from_nv(&config))
|
|
{
|
|
BT_DRV_TRACE(0,"Calibration value in NV:");
|
|
|
|
for (uint32_t range = 0;range < BT_FREQENCY_RANGE_NUM;range++)
|
|
{
|
|
BT_DRV_TRACE(3,"%d: 0x%x - 0x%x", range,
|
|
config.gain_cal_val[range],
|
|
config.phase_cal_val[range]);
|
|
}
|
|
uint32_t addr = 0xd0310000;
|
|
int chs = 27;
|
|
|
|
for (uint32_t range = 0;range < 3;range++)
|
|
{
|
|
if (0 == range)
|
|
{
|
|
addr = 0xd0310034;
|
|
chs= 27;
|
|
addr = addr-13*4;
|
|
}
|
|
|
|
for (uint32_t ch=0;ch < chs;ch++)
|
|
{
|
|
bt_iqimb_add_mismatch(0,
|
|
config.gain_cal_val[range],
|
|
config.phase_cal_val[range],
|
|
0,0,addr);
|
|
addr = addr +4;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
btIqCalibration();
|
|
uint32_t lock = nv_record_pre_write_operation();
|
|
nv_record_get_extension_entry_ptr()->btIqCalConfig = config;
|
|
nv_record_post_write_operation(lock);
|
|
|
|
nv_record_extension_update();
|
|
}
|
|
}
|
|
|
|
void bt_IQ_DC_Mismatch_Correction_Release()
|
|
{
|
|
int phase_mis_base;
|
|
int gain_mis_base;
|
|
int Energy,Energy1;
|
|
uint32_t time_start = hal_sys_timer_get();
|
|
int fftsize;
|
|
uint32_t addr = 0xd0310000;
|
|
int chs = 27;
|
|
IQMismatchPreprocessState *st;
|
|
syspool_init();
|
|
syspool_get_buff(&g_medMemPool, MED_MEM_POOL_SIZE);
|
|
med_heap_init(&g_medMemPool[0], MED_MEM_POOL_SIZE);
|
|
//config gpadc 26m
|
|
READ_REG(0xd0310000,0x0);
|
|
#ifdef DCCalib
|
|
int dc_i_base,dc_q_base;
|
|
fftsize = 1024;
|
|
st = IQMismatchPreprocessState_init(fftsize);
|
|
Tblgen(st->Table0,st->Table1,st->Table2,fftsize);
|
|
DC_correction(st,&dc_i_base,&dc_q_base,fftsize);
|
|
bt_Txdc_cal_set(0,dc_i_base); //set cal DC I
|
|
bt_Txdc_cal_set(1,dc_q_base);
|
|
//val = READ_REG(0xd0350348,0x0);
|
|
//BT_DRV_TRACE(1,"0xd0350348 = 0x%08x",val);
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
// BT_DRV_TRACE(3,"dc cal done!!! dc_i = %d ,dc_q = %d,Energy1 = %d",dc_i_base,dc_q_base,Energy1);
|
|
if(Energy1>200)
|
|
{
|
|
DC_correction(st,&dc_i_base,&dc_q_base,fftsize);
|
|
bt_Txdc_cal_set(0,dc_i_base); //set cal DC I
|
|
bt_Txdc_cal_set(1,dc_q_base);
|
|
//val = READ_REG(0xd0350348,0x0);
|
|
//BT_DRV_TRACE(1,"0xd0350348 = 0x%08x",val);
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
}
|
|
if(Energy1>200)
|
|
{
|
|
DC_correction(st,&dc_i_base,&dc_q_base,fftsize);
|
|
bt_Txdc_cal_set(0,dc_i_base); //set cal DC I
|
|
bt_Txdc_cal_set(1,dc_q_base);
|
|
//val = READ_REG(0xd0350348,0x0);
|
|
//BT_DRV_TRACE(1,"0xd0350348 = 0x%08x",val);
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
}
|
|
#endif
|
|
// BT_DRV_TRACE(1,"use time: %d ticks", (hal_sys_timer_get()-time_start));
|
|
// BT_DRV_TRACE(3,"dc cal done!!! dc_i = %d ,dc_q = %d,Energy1 = %d",dc_i_base,dc_q_base,Energy1);
|
|
fftsize = 1024;
|
|
st = IQMismatchPreprocessState_init(fftsize);
|
|
Tblgen_iq(st->Table0,st->Table1,st->Table2,fftsize);
|
|
BT_DRV_TRACE(1,"use time: %d ms", __TICKS_TO_MS(hal_sys_timer_get()-time_start));
|
|
for (int k = 0; k<3; k++)
|
|
{
|
|
if(k==0)
|
|
{
|
|
WRITE_REG(0x0,0xD02201E4,0x0);
|
|
WRITE_REG(0x000A000D,0xD02201E4,0x0);
|
|
//osDelay(1000);
|
|
addr = 0xd0310034;
|
|
}
|
|
else if(k==1)
|
|
{
|
|
WRITE_REG(0x0,0xD02201E4,0x0);
|
|
WRITE_REG(0x000A0027,0xD02201E4,0x0);
|
|
addr = 0xd031009c;
|
|
}
|
|
else
|
|
{
|
|
WRITE_REG(0x0,0xD02201E4,0x0);
|
|
WRITE_REG(0x000A0041,0xD02201E4,0x0);
|
|
addr = 0xd0310104;
|
|
}
|
|
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,0,fftsize,addr);
|
|
phase_mis_base = IQ_Phase_Mismatch_Correction(st,gain_mis_base,fftsize,addr);
|
|
bt_iqimb_add_mismatch(0,gain_mis_base,phase_mis_base,0,0,addr); //no mismatch
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
|
|
if(Energy1>500)
|
|
{
|
|
if(ABS(gain_mis_base)>ABS(phase_mis_base))
|
|
{
|
|
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,0,fftsize,addr);
|
|
phase_mis_base = IQ_Phase_Mismatch_Correction(st,gain_mis_base,fftsize,addr);
|
|
}
|
|
else
|
|
{
|
|
phase_mis_base = IQ_Phase_Mismatch_Correction(st,0,fftsize,addr);
|
|
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,phase_mis_base,fftsize,addr);
|
|
}
|
|
|
|
bt_iqimb_add_mismatch(0,gain_mis_base,phase_mis_base,0,0,addr); //no mismatch
|
|
// BT_DRV_TRACE(1,"use time: %d ticks", (hal_sys_timer_get()-time_start));
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
|
|
}
|
|
|
|
if(Energy1>500)
|
|
{
|
|
if(ABS(gain_mis_base)>ABS(phase_mis_base))
|
|
{
|
|
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,0,fftsize,addr);
|
|
phase_mis_base = IQ_Phase_Mismatch_Correction(st,gain_mis_base,fftsize,addr);
|
|
}
|
|
else
|
|
{
|
|
phase_mis_base = IQ_Phase_Mismatch_Correction(st,0,fftsize,addr);
|
|
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,phase_mis_base,fftsize,addr);
|
|
}
|
|
// BT_DRV_TRACE(1,"use time: %d ticks", (hal_sys_timer_get()-time_start));
|
|
bt_iqimb_dma_enable(st->M0data,fftsize);
|
|
while(1)
|
|
{
|
|
if(iqimb_dma_status==0)
|
|
break;
|
|
}
|
|
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
|
|
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
|
|
}
|
|
if(k==0)
|
|
{
|
|
chs= 27;
|
|
addr = addr-13*4;
|
|
}
|
|
else if(k==1)
|
|
{
|
|
chs = 26;
|
|
addr = addr - 12*4;
|
|
}
|
|
else
|
|
{
|
|
chs = 26;
|
|
addr = addr -12*4;
|
|
}
|
|
|
|
bt_update_local_iq_calibration_val(k, gain_mis_base, phase_mis_base);
|
|
|
|
for (int ch=0; ch<chs; ch++)
|
|
{
|
|
bt_iqimb_add_mismatch(0,gain_mis_base,phase_mis_base,0,0,addr); //no mismatch
|
|
// osDelay(1);
|
|
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
|
|
addr = addr +4;
|
|
}
|
|
BT_DRV_TRACE(1,"use time: %d ms", __TICKS_TO_MS(hal_sys_timer_get()-time_start));
|
|
//BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr-4,phase_mis_base,gain_mis_base,Energy1);
|
|
}
|
|
}
|
|
|
|
int bt_iqimb_test_ex (int mismatch_type)
|
|
{
|
|
|
|
bt_IQ_DC_Mismatch_Correction_Release();
|
|
|
|
return 1;
|
|
}
|
|
|
|
#endif//TX_IQ_CAL
|
|
#endif
|