function.c

基于TMS320F2812的AMR+RS编解码+OFDM调制解调源程序。是stand alone运行版本

#include <stdio.h>
#include <math.h>
#include "Setting.H"
#include "RS.H"
#include "costable.h"
#include "sintable.h"

long int * point_Real;
long int * point_Imag;
static WordType i,j,k;
//static float TmpR, TmpI;
DType * point_DataIn;
DType * point_DataOut;
long int TmpR,TmpI,UR,UI;
/*
 Input is in reverse order, output is in natural order;
 * FFT is sampling on the time, base on radix-2;
 */
 
void FFT(long int Real[], long int Imag[], WordType sign)
{
	WordType l, n1, n2;
	long int  index,index2;
	//long int * pRealk,* pReali,* pImagk,* pImagi,Realk,Reali,Imagk,Imagi;

	point_Real=Real;
    point_Imag=Imag;

	/* Compute the power M: 2^M = FFTSize 
	for (M = 0, i = 1; ;)
	{
		M += 1;
		i *= 2;
		if (i == FFTSize)
		{
			break;
		}
	}*/

	/* Exchange data position in reverse order */
	for (j=0, i=0; i < FFTSize - 1; i++)
	{
		if (i < j)
		{
			asm(" push xar0");
            asm(" push xar1");
            asm(" push xar4");
            asm(" push p");    
            asm(" movl xar4,@_point_Real");
            asm(" movl xar6,@_point_Imag");
            asm(" mov  acc,@_i << 1 ");
            asm(" movz ar0,al ");
            asm(" mov  acc,@_j << 1 ");
            asm(" movz ar1,al"); 
            asm(" movl acc,*+xar4[ar0]");
            asm(" movl p,*+xar4[ar1]");
            asm(" movl *+xar4[ar1],acc");
            asm(" movl *+xar4[ar0],p");
            asm(" movl acc,*+xar6[ar0]");
            asm(" movl p,*+xar6[ar1]");
            asm(" movl *+xar6[ar1],acc");
            asm(" movl *+xar6[ar0],p"); 
            asm(" pop  p");
            asm(" pop  xar4");
            asm(" pop  xar1");
            asm(" pop  xar0");   
			/*TmpR = Real[j];
			TmpI = Imag[j];
			Real[j] = Real[i];
			Imag[j] = Imag[i];
			Real[i] = TmpR;
			Imag[i] = TmpI;*/
		}
		k = FFTSize / 2;
		while(k < (j + 1))
		{
			j -= k;
			k /= 2;
		}
		j += k;
	}
 
 	n1 = 1;
	for(l = 1; l <= 6; l++)
	{
		n2 = n1;
		n1 = 2 * n1;
		index = 1<<(6-l);
		for(j = 0; j < n2; j++)
		{
			index2 = j*index;
			UR = cos_tab[index2];
			UI = -sign*sin_tab[index2];
			for(i = j; i < FFTSize; i += n1)
			{
				k = i + n2;
				asm(" push xar0");
           		asm(" push xar1");
            	asm(" push xar4");
            	asm(" push xar6");
            	asm(" push p");    
				asm(" movl  xar4,@_point_Real");
            	asm(" movl  xar6,@_point_Imag");
            	asm(" mov   acc,@_k << 1");
            	asm(" movz  ar0,al");
            	asm(" movl  XT,@_UI");
            	asm(" impyl acc,XT,*+xar6[ar0]");
            	asm(" movl  xar5,acc");
            	asm(" movl  XT,@_UR");            	
            	asm(" impyl acc,XT,*+xar4[ar0]");
            	asm(" subl acc,xar5");
            	//asm(" add acc,#1");
            	asm(" SFR   acc,#14");
            	asm(" movl  @_TmpR, acc");
               	asm(" mov   acc,@_k << 1");
            	asm(" movz  ar0,al");
            	asm(" movl  XT,@_UR");
            	asm(" impyl acc,XT,*+xar6[ar0]");
            	asm(" movl  xar5,acc");
            	asm(" movl  XT,@_UI");
            	asm(" impyl acc,XT,*+xar4[ar0]");
            	asm(" addl  acc,xar5");
            	asm(" SFR   acc,#14");
            	asm(" movl  @_TmpI, acc");
            	asm(" mov   acc,@_i << 1");
            	asm(" movz  ar1,al");
            	
            	asm(" movl  acc,*+xar4[ar1]");
            	asm(" subl  acc,@_TmpR");
            	asm(" movl  *+xar4[ar0],acc");
            	
            	asm(" movl  acc,*+xar6[ar1]");
            	asm(" subl  acc,@_TmpI");
            	asm(" movl  *+xar6[ar0],acc");
            	
            	asm(" movl  acc,@_TmpR");
            	asm(" addl  *+xar4[ar1],acc");
            	
            	asm(" movl  acc,@_TmpI");
            	asm(" addl  *+xar6[ar1],acc");
			 	asm(" pop  p");
            	asm(" pop  xar4");
            	asm(" pop  xar6");
            	asm(" pop  xar1");
            	asm(" pop  xar0");  
			 /* TmpR = (UR * Real[k] - UI * Imag[k])>>14;
				TmpI = (UR * Imag[k] + UI * Real[k])>>14;
				Real[k] = Real[i] - TmpR;
				Imag[k] = Imag[i] - TmpI;
				Real[i] += TmpR;
				Imag[i] += TmpI;*/
			}
		}
	}

	/* Adjust the amplitude if applying IFFT */
	if (sign == -1)
	{
		for(i = 0; i < FFTSize; i++)
		{
			Real[i] = Real[i] >> 6;
			Imag[i] = Imag[i] >> 6;
		}
	}
}

/* CONVBASE Converts the number base of the input data (DataIn):
 *   ConvBase(DType DataIn[], DType DataOut[], DType DataLen, DType sign)
 * From the current base (CurrBase) to the new base required (NewBase);
 * If the new base is less than the old base, e.g
 * NewBase = 2 and CurrBase = 8, the DataOut will have more
 * data words. For the above example DataOut will be 4 times
 * longer than DataIn;
 * The actual base used = 2^NewBase, thus NewBase = 8, is base 256;
 * E.g. DataIn = [ 2 64 20 ] with CurrBase = 8, NewBase = 2
 * DataOut = [0 0 0 2, 1 0 0 0, 0 1 1 0]
 * Note : Both the input and output data is in a serial format;
 * DataLen is the length of DataIn[];
 * Sign represents the ConvBase() function: 1 -> NewBase < CurrBase
 *                                          0 -> NewBase > CurrBase;
 *                                          Other -> No change;
 */
void ConvBase(DType DataIn[], DType DataOut[], WordType DataLen, WordType Sign)
{
    WordType j, k;
        
    point_DataIn=DataIn;
    point_DataOut=DataOut; 
	
	if (Sign == 1)
	{
		/* Convert the input data into the smaller base required */
		for (i = 0; i < DataLen; i++)
		{
			point_DataOut = DataOut + i * RSInforBit / CodewordBit;
			point_DataIn = DataIn + i;
			for (j = 0; j < (RSInforBit / CodewordBit); j++)
			{
				/*DataOut[i * RSInforBit / CodewordBit + j] =
					 DataIn[i] & (DType)((1 << CodewordBit) - 1);
				DataIn[i] >>= CodewordBit;*/
				*point_DataOut = *point_DataIn & (DType)((1 << CodewordBit) - 1);
				*point_DataIn >>= CodewordBit;
				point_DataOut++;
			}
		}
	}
	else if (Sign == 0)
	{
		/* Convert back to larger base */
		j = 0;
		k = 0;
		for (i = 0; i < DataLen; i++)
		{
			if (k == 0)
			{
				DataOut[j] = 0;
			}
			DataOut[j] += (DType) (1 << (CodewordBit * k)) * DataIn[i];
			if (k == (RSInforBit / CodewordBit - 1))
			{
				j++;
				k = 0;
			}
			else
			{
				k++;
			}
		}
	}
	else
	{
		/* No change of base */
		asm(" push xar0");
		asm(" push xar6");
		asm(" push xar7");
        asm(" movl xar6,@_point_DataIn");
        asm(" movl xar7,@_point_DataOut");
		for (i = 0; i < DataLen; i++)
		{
		    asm(" movz ar0,@_i");
		    asm(" mov al,*+xar6[ar0]"); 
            asm(" mov *+xar7[ar0],al");
			//DataOut[i] = DataIn[i];
		}
	    asm(" pop xar7");
	    asm(" pop xar6");
	    asm(" pop xar0");
	}
}

/* Convert 2-bit binary code to Gray code */
void Gray2(DType DataIn[], WordType Length)
{
//	WordType i;
	for (i = 0; i < Length; i++)
	{
		switch(DataIn[i])
		{
		/*case 0:
		    DataIn[i] = 0;
			break;
		case 1:
			DataIn[i] = 1;
			break;
*/
		case 2:
			DataIn[i] = 3;
			break;
		case 3:
			DataIn[i] = 2;
			break;
		default: 
		    break;	
		}
	}
}

/* Convert the source encoded data to required format */
void SrcToChan(int SrcData[], DType ReqData[], WordType SymbolMark[], WordType BitMark[])
{
	WordType i;
	for (i = 0; i < SrcVector; i++)
	{
		if (BitMark[0] == 0)
		{
			ReqData[SymbolMark[0]] = SrcData[i];
		}
		else
		{
			ReqData[SymbolMark[0]] += SrcData[i] * (1 << BitMark[0]);
		}
		if (BitMark[0] == 5)
		{
			BitMark[0] = 0;
			SymbolMark[0]++;
		}
		else
		{
			BitMark[0]++;
		}
	}
}

/* Convert the channel decoded data to required source format */
void ChanToSrc(DType ChannelData[], short SrcData[], WordType PosMark[])
{
	WordType i, j;
	DType *pChaData;
	short *pSrcData;
	
	pChaData = ChannelData;
	pSrcData = &SrcData[PosMark[0]];
	
	for (i = 0; i < RSInforSym; i++)
	{
		for (j = 0; j < RSInforBit; j++)
		{
			*pSrcData = (short) (*pChaData & 1);
			*pChaData >>= 1;
			(PosMark[0])++;
			pSrcData++;
		}
		pChaData++;
	}
}
/* Round the real data to nearest integer */
WordType Round(float RealData)
{
	WordType a;
	
	a = 1;
	
	if (RealData < 0)
	{
		a = -1;
		RealData = 0 - RealData;
	}
	if ((RealData < 0.5) || ((WordType)RealData != (WordType)(RealData - 0.5)))
	{
		return ((WordType)RealData * a);
	}
	else
	{
		return (((WordType)RealData + 1) * a);
	}
}

/* Generates a pseudorandom number */
unsigned short Random(unsigned long int *Seed, unsigned short RandMax)
{
	asm(" spm 0");
	*Seed= 2045 * (*Seed) + 1;
	*Seed= *Seed - (*Seed / 104877) * 104877;
	return (WordType)(*Seed & RandMax);
}

// Fast arctan2
long int arctan2(long int y, long int x)
{
	long int abs_y, angle;

	if      (y<0) {abs_y = -y;}
	else if (y>0) {abs_y =  y;}
	else          {abs_y =  1;}	// kludge to prevent 0/0 condition

   if (x>=0){angle = QtrPIQ15 -  (x - abs_y) * QtrPIQ15 / (x + abs_y);}
   else     {angle = Qtr3PIQ15 - (x + abs_y) * QtrPIQ15 / (abs_y - x);}
   if (y<0) return(PI2Q15-angle);     // negate if in quad III or IV
   else     return(angle);
}