jens
290fd5f5e0
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/radio@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
592 lines
9.9 KiB
C
Executable File
592 lines
9.9 KiB
C
Executable File
// --------------------------------------------------------------
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "radio_types.h"
|
|
#include "real.h"
|
|
#include "cpx.h"
|
|
|
|
// --------------------------------------------------------------
|
|
// Basic Functions
|
|
// --------------------------------------------------------------
|
|
|
|
cpx_t CpxConjS(cpx_t v1)
|
|
{
|
|
register cpx_t temp;
|
|
|
|
temp.real = v1.real;
|
|
temp.imag = -v1.imag;
|
|
|
|
return temp;
|
|
|
|
}
|
|
|
|
cpx_t CpxAddS(cpx_t v1, cpx_t v2)
|
|
{
|
|
register cpx_t temp;
|
|
|
|
temp.real = v1.real + v2.real;
|
|
temp.imag = v1.imag + v2.imag;
|
|
|
|
return temp;
|
|
}
|
|
|
|
cpx_t CpxSubS(cpx_t v1, cpx_t v2)
|
|
{
|
|
register cpx_t temp;
|
|
|
|
temp.real = v1.real - v2.real;
|
|
temp.imag = v1.imag - v2.imag;
|
|
|
|
return temp;
|
|
}
|
|
|
|
cpx_t CpxMulS(cpx_t v1, cpx_t v2)
|
|
{
|
|
register cpx_t t_0;
|
|
register cpx_t t_1;
|
|
|
|
t_0.real = v1.real*v2.real;
|
|
t_1.real = v1.imag*v2.imag;
|
|
t_0.imag = v1.real*v2.imag;
|
|
t_1.imag = v1.imag*v2.real;
|
|
|
|
t_0.real -= t_1.real;
|
|
t_0.imag += t_1.imag;
|
|
|
|
return t_0;
|
|
}
|
|
|
|
cpx_t CpxScaleRealS(cpx_t v1, radio_float_t v2)
|
|
{
|
|
register cpx_t temp;
|
|
|
|
temp.real = v1.real*v2;
|
|
temp.imag = v1.imag*v2;
|
|
|
|
return temp;
|
|
}
|
|
|
|
cpx_t CpxScaleComplexS(cpx_t x, cpx_t gain)
|
|
{
|
|
register cpx_t temp;
|
|
|
|
temp.real = x.real*gain.real;
|
|
temp.imag = x.imag*gain.imag;
|
|
|
|
return temp;
|
|
}
|
|
|
|
cpx_t CpxFromReal(radio_float_t v1)
|
|
{
|
|
return Cpx(v1, v1);
|
|
}
|
|
|
|
cpx_t Cpx(radio_float_t real, radio_float_t imag)
|
|
{
|
|
cpx_t res;
|
|
|
|
res.real = real;
|
|
res.imag = imag;
|
|
|
|
return res;
|
|
}
|
|
|
|
radio_float_t CpxMagS(cpx_t v1)
|
|
{
|
|
radio_float_t mag;
|
|
|
|
mag = (radio_float_t)sqrt(v1.real*v1.real + v1.imag*v1.imag);
|
|
|
|
return mag;
|
|
}
|
|
|
|
radio_float_t CpxPhiS(cpx_t v1)
|
|
{
|
|
return (radio_float_t)atan2(v1.real, v1.imag);
|
|
}
|
|
|
|
cpx_t CpxMinS(cpx_t x1, cpx_t x2)
|
|
{
|
|
return Cpx(dmin(x1.real, x2.real), dmin(x1.imag, x2.imag));
|
|
}
|
|
|
|
cpx_t CpxMaxS(cpx_t x1, cpx_t x2)
|
|
{
|
|
return Cpx(dmax(x1.real, x2.real), dmax(x1.imag, x2.imag));
|
|
}
|
|
|
|
radio_float_t cpxPowerDB(cpx_t v1, radio_float_t preScale)
|
|
{
|
|
return powerDB(CpxMagS(v1), preScale);
|
|
}
|
|
|
|
// --------------------------------------------------------------
|
|
// Complex Functions
|
|
// --------------------------------------------------------------
|
|
void CpxCopy(cpx_t *pSrc, cpx_t *pDst, uint32_t len)
|
|
{
|
|
memcpy(pDst, pSrc, len*sizeof(cpx_t));
|
|
}
|
|
|
|
void CpxConj(cpx_t *pSrcDst, uint32_t len)
|
|
{
|
|
uint32_t i;
|
|
for(i=0; i < len; i++)
|
|
pSrcDst[i] = CpxConjS(pSrcDst[i]);
|
|
}
|
|
|
|
void CpxAddB8(cpx_t *pV1, cpx_t *pV2, cpx_t *pRes)
|
|
{
|
|
uint32_t i;
|
|
|
|
radio_float_t r0[8];
|
|
radio_float_t i0[8];
|
|
|
|
radio_float_t vr0[8];
|
|
radio_float_t vr1[8];
|
|
radio_float_t vi0[8];
|
|
radio_float_t vi1[8];
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
vr0[i] = pV1[i].real;
|
|
vr1[i] = pV2[i].real;
|
|
vi0[i] = pV1[i].imag;
|
|
vi1[i] = pV2[i].imag;
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i] + vr1[i];
|
|
i0[i] = vi0[i] + vi1[i];
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes[i].real = r0[i];
|
|
pRes[i].imag = i0[i];
|
|
}
|
|
|
|
}
|
|
|
|
void CpxAdd(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t len)
|
|
{
|
|
uint32_t i;
|
|
|
|
for(i=0; i < len-8; i += 8)
|
|
{
|
|
CpxAddB8(&pSrc[i], &pSrcDst[i], &pSrcDst[i]);
|
|
}
|
|
|
|
for(i=i; i < len; i++)
|
|
{
|
|
pSrcDst[i].real += pSrc[i].real;
|
|
pSrcDst[i].imag += pSrc[i].imag;
|
|
}
|
|
}
|
|
|
|
void CpxSub(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t len)
|
|
{
|
|
uint32_t i;
|
|
for(i=0; i < len; i++)
|
|
pSrcDst[i] = CpxSubS(pSrc[i], pSrcDst[i]);
|
|
}
|
|
|
|
void CpxMulB8(cpx_t *pV1, cpx_t *pV2, cpx_t *pRes)
|
|
{
|
|
uint32_t i;
|
|
radio_float_t r0[8];
|
|
radio_float_t r1[8];
|
|
|
|
radio_float_t vr0[8];
|
|
radio_float_t vr1[8];
|
|
radio_float_t vi0[8];
|
|
radio_float_t vi1[8];
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
vr0[i] = pV1[i].real;
|
|
vr1[i] = pV2[i].real;
|
|
vi0[i] = pV1[i].imag;
|
|
vi1[i] = pV2[i].imag;
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*vr1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r1[i] = vi0[i]*vi1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes[i].real = r0[i] - r1[i];
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*vi1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r1[i] = vi0[i]*vr1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes[i].imag = r0[i] + r1[i];
|
|
}
|
|
|
|
}
|
|
|
|
void CpxMulAddB8(cpx_t *pV1, cpx_t *pV2, cpx_t *pRes)
|
|
{
|
|
uint32_t i;
|
|
radio_float_t r0[8];
|
|
radio_float_t r1[8];
|
|
|
|
radio_float_t vr0[8];
|
|
radio_float_t vr1[8];
|
|
radio_float_t vi0[8];
|
|
radio_float_t vi1[8];
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
vr0[i] = pV1[i].real;
|
|
vr1[i] = pV2[i].real;
|
|
vi0[i] = pV1[i].imag;
|
|
vi1[i] = pV2[i].imag;
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*vr1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r1[i] = vi0[i]*vi1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] -= r1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes->real += r0[i];
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*vi1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r1[i] = vi0[i]*vr1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] += r1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes->imag += r0[i];
|
|
}
|
|
|
|
}
|
|
void CpxScaleB8(cpx_t *v1, cpx_t v2, cpx_t *pRes)
|
|
{
|
|
uint32_t i;
|
|
radio_float_t r0[8];
|
|
radio_float_t r1[8];
|
|
|
|
radio_float_t vr0[8];
|
|
radio_float_t vi0[8];
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
vr0[i] = v1[i].real;
|
|
vi0[i] = v1[i].imag;
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*v2.real;
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r1[i] = vi0[i]*v2.imag;
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] -= r1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes[i].real = r0[i];
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*v2.imag;
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r1[i] = vi0[i]*v2.real;
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] += r1[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes[i].imag = r0[i];
|
|
}
|
|
}
|
|
|
|
void CpxMulRealB8(cpx_t *v1, radio_float_t *v2, cpx_t *pRes)
|
|
{
|
|
uint32_t i;
|
|
|
|
radio_float_t r0[8];
|
|
radio_float_t i0[8];
|
|
|
|
radio_float_t vr0[8];
|
|
radio_float_t vi0[8];
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
vr0[i] = v1[i].real;
|
|
vi0[i] = v1[i].imag;
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*v2[i];
|
|
i0[i] = vi0[i]*v2[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes[i].real = r0[i];
|
|
pRes[i].imag = i0[i];
|
|
}
|
|
}
|
|
|
|
void CpxMulRealAddB8(cpx_t *v1, radio_float_t *v2, cpx_t *pRes)
|
|
{
|
|
uint32_t i;
|
|
|
|
radio_float_t r0[8];
|
|
radio_float_t i0[8];
|
|
|
|
radio_float_t vr0[8];
|
|
radio_float_t vi0[8];
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
vr0[i] = v1[i].real;
|
|
vi0[i] = v1[i].imag;
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
r0[i] = vr0[i]*v2[i];
|
|
i0[i] = vi0[i]*v2[i];
|
|
}
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes->real += r0[i];
|
|
pRes->imag += i0[i];
|
|
}
|
|
}
|
|
|
|
void CpxAccumB8(cpx_t *pV1, cpx_t *pRes)
|
|
{
|
|
uint32_t i;
|
|
|
|
radio_float_t vr0[8];
|
|
radio_float_t vi0[8];
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
vr0[i] = pV1[i].real;
|
|
vi0[i] = pV1[i].imag;
|
|
}
|
|
|
|
for (i=0; i < 8; i++)
|
|
{
|
|
pRes->real += vr0[i];
|
|
pRes->imag += vi0[i];
|
|
}
|
|
|
|
}
|
|
|
|
cpx_t CpxMulAdd(cpx_t sum, cpx_t *pSrc1, cpx_t *pSrc2, uint32_t len)
|
|
{
|
|
uint32_t i;
|
|
|
|
for (i=0; i < (len-8); i += 8)
|
|
{
|
|
CpxMulAddB8(&pSrc1[i], &pSrc2[i], &sum);
|
|
}
|
|
|
|
for (i=i; i < len; i++)
|
|
{
|
|
sum = CpxAddS(sum, CpxMulS(pSrc1[i], pSrc2[i]));
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
cpx_t CpxMulRealAdd(cpx_t sum, cpx_t *pSrc1, radio_float_t *pSrc2, uint32_t len)
|
|
{
|
|
uint32_t i;
|
|
|
|
for (i=0; i < (len-8); i += 8)
|
|
{
|
|
CpxMulRealAddB8(&pSrc1[i], &pSrc2[i], &sum);
|
|
}
|
|
|
|
for (i=i; i < len; i++)
|
|
{
|
|
sum = CpxAddS(sum, CpxScaleRealS(pSrc1[i], pSrc2[i]));
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
void CpxMul(cpx_t *pSrc1, cpx_t *pSrc2, cpx_t *pDst, uint32_t len)
|
|
{
|
|
|
|
uint32_t i;
|
|
|
|
for (i=0; i < (len-8); i += 8)
|
|
{
|
|
CpxMulB8(&pSrc1[i], &pSrc2[i], &pDst[i]);
|
|
}
|
|
|
|
for (i=i; i < len; i++)
|
|
{
|
|
pDst[i] = CpxMulS(pSrc1[i], pSrc2[i]);
|
|
}
|
|
}
|
|
|
|
void CpxScale(cpx_t *pSrc1, cpx_t src2, cpx_t *pDst, uint32_t len)
|
|
{
|
|
|
|
uint32_t i;
|
|
|
|
for (i=0; i < (len-8); i += 8)
|
|
{
|
|
CpxScaleB8(&pSrc1[i], src2, &pDst[i]);
|
|
}
|
|
|
|
for (i=i; i < len; i++)
|
|
{
|
|
pDst[i] = CpxMulS(pSrc1[i], src2);
|
|
}
|
|
}
|
|
|
|
cpx_t csum(cpx_t *pSrc, int len)
|
|
{
|
|
cpx_t sum = Cpx(0,0);
|
|
|
|
while(len--)
|
|
{
|
|
sum.real += (*(pSrc)).real;
|
|
sum.imag += (*(pSrc++)).imag;
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
cpx_t cmean(cpx_t *pSrc, int len)
|
|
{
|
|
cpx_t y;
|
|
|
|
y = csum(pSrc, len);
|
|
y.real /= len;
|
|
y.imag /= len;
|
|
|
|
return y;
|
|
}
|
|
|
|
// --------------------------------------------------------------
|
|
#include <xmmintrin.h>
|
|
|
|
void _cpxMulB4
|
|
(
|
|
radio_float_t pV1_real[_cpxMulB4_blockSize], radio_float_t pV1_imag[_cpxMulB4_blockSize],
|
|
radio_float_t pV2_real[_cpxMulB4_blockSize], radio_float_t pV2_imag[_cpxMulB4_blockSize],
|
|
radio_float_t pRes_real[_cpxMulB4_blockSize], radio_float_t pRes_imag[_cpxMulB4_blockSize])
|
|
{
|
|
uint32_t i;
|
|
radio_float_t r0[_cpxMulB4_blockSize];
|
|
radio_float_t r1[_cpxMulB4_blockSize];
|
|
radio_float_t r2[_cpxMulB4_blockSize];
|
|
radio_float_t r3[_cpxMulB4_blockSize];
|
|
|
|
for (i=0; i < _cpxMulB4_blockSize; i++)
|
|
{
|
|
r0[i] = pV1_real[i]*pV2_real[i];
|
|
}
|
|
for (i=0; i < _cpxMulB4_blockSize; i++)
|
|
{
|
|
r1[i] = pV1_imag[i]*pV2_imag[i];
|
|
}
|
|
for (i=0; i < _cpxMulB4_blockSize; i++)
|
|
{
|
|
r2[i] = pV1_real[i]*pV2_imag[i];
|
|
}
|
|
for (i=0; i < _cpxMulB4_blockSize; i++)
|
|
{
|
|
r3[i] = pV1_imag[i]*pV2_real[i];
|
|
}
|
|
for (i=0; i < _cpxMulB4_blockSize; i++)
|
|
{
|
|
pRes_real[i] = r0[i] - r1[i];
|
|
}
|
|
for (i=0; i < _cpxMulB4_blockSize; i++)
|
|
{
|
|
pRes_imag[i] = r2[i] + r3[i];
|
|
}
|
|
}
|
|
|
|
void _cpxMulB1
|
|
(
|
|
radio_float_t *pV1_real, radio_float_t *pV1_imag,
|
|
radio_float_t *pV2_real, radio_float_t *pV2_imag,
|
|
radio_float_t *pRes_real, radio_float_t *pRes_imag)
|
|
{
|
|
|
|
uint32_t i;
|
|
radio_float_t r0[_cpxMulB1_blockSize];
|
|
radio_float_t r1[_cpxMulB1_blockSize];
|
|
radio_float_t r2[_cpxMulB1_blockSize];
|
|
radio_float_t r3[_cpxMulB1_blockSize];
|
|
|
|
for (i=0; i < _cpxMulB1_blockSize; i++)
|
|
{
|
|
r0[i] = pV1_real[i]*pV2_real[i];
|
|
}
|
|
for (i=0; i < _cpxMulB1_blockSize; i++)
|
|
{
|
|
r1[i] = pV1_imag[i]*pV2_imag[i];
|
|
}
|
|
for (i=0; i < _cpxMulB1_blockSize; i++)
|
|
{
|
|
r2[i] = pV1_real[i]*pV2_imag[i];
|
|
}
|
|
for (i=0; i < _cpxMulB1_blockSize; i++)
|
|
{
|
|
r3[i] = pV1_imag[i]*pV2_real[i];
|
|
}
|
|
for (i=0; i < _cpxMulB1_blockSize; i++)
|
|
{
|
|
pRes_real[i] = r0[i] - r1[i];
|
|
}
|
|
for (i=0; i < _cpxMulB1_blockSize; i++)
|
|
{
|
|
pRes_imag[i] = r2[i] + r3[i];
|
|
}
|
|
}
|