Initial import

git-svn-id: http://moon:8086/svn/software/trunk/libsrc/radio@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
2014-07-19 07:44:42 +00:00
commit 290fd5f5e0
36 changed files with 13976 additions and 0 deletions
@@ -0,0 +1,591 @@
+// --------------------------------------------------------------
+#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];
+	}
+}