src_flt.asm

在blankfin中 实现src 的程序

/* 	File: src_flt.asm Version 0.1


fundemental structure order:

1.   stage data handle
2.   half band flag (0,1, or 2)
3.   number of up stages
4.   pivot flag (0 or 1)
5.   number of down stages
6.   number of stages (total)
7.   number of input samples per block
8.   number of output samples per block


	P0 -> fundamental structure
	P1 -> input samples
	P2 -> output samples
	P3 -> memory storage and retreival
	P4 = temporary pointer
	P5 = loop counter

	R0 = Loop counters
	R1 = temporary storage
	R2 = Loop counters
	R3 = Shift count
	R4 = inner loop calculations
	R5 = inner loop calculations
	R6 = temporary storage
	R7 = temporary storage

	I0 = dedicated to input buffer 'inx'
	I1 = general use...reading 'inputData' plus others
	I2 = general use...reading 'inx' for output data
	I3 = general use...

 
   Input Data Structure (VAR_SIZE words)
   	AIS: address of input signal (circular), updated after return,
   	SIS: circular size of AIS,
   	AOS: address of output signal (circular), updated after return,
   	SOS: circular size of AOS,
   	AFA: address of filter array,
   	LEN: poly-phase filter length,
    UPR: up sample rate >= 2,
	DNR: down sample rate = 1 is assumed
   	NIS: number of input signals
	NOS: number of output signals
	SHF: number of shift counter, 0 or 1
   	

*/



.SECTION	L1_data_a;
.align 4;

.byte4 pt_fundst;			// pointer to fundamental structure
.byte4 pt2_fundst;			// pointer to fundamental structure
.byte4 st_handle;			// pointer to stage data handle
.byte2 inputs;				// number of inputs
.byte2 outputs;				// number of outputs
.byte2 diff_offset;			// Offset difference
		

.GLOBAL _src_flt;

.SECTION program;

_src_flt:

	[--SP]=(R7:4,P5:3);		// Push R7...
	P1 = R0;				// Address of input data
	P2 = R1;				// Address of output data
	P0 = [SP+40];			// Address of fundemental structure

	p3.l = diff_offset;
	p3.h = diff_offset;
	w[p3] = r2;				// save DOFS3 (difference offset to strip filter delay off of final buffer

    p3.l = pt_fundst;		// p3 -> to fundemental structure
	p3.h = pt_fundst;
	[p3] = p0;				// save fundemental stage pointer in memory pointed at by p3

#ifdef BUFIN

	p5 = 24;				// 6*4 = 24 bytes (post increment points to number input samples per block)
	r6 = [p0++p5];
	p4 = r6;				// p4 -> stage handle

	r6 = [p0++];			// r6 = number of inputs samples per block
    p3.l = inputs;			// p3 -> to number of input samples per block
	p3.h = inputs;
	w[p3] = r6;				// save number of input samples per block


	
	r6 = [p0++];			// r6 = number of output samples per block
	p3.l = outputs;			// p3 -> to number of output samples per block
	p3.h = outputs;
	w[p3] = r6;				// save number of output samples per block
	p0 = [p4++];			// p0 -> fist data structure
/******** IPDC comment	 *******/
//	r6 = [p0++];			// r6 -> first input buffer 'inx'
//	i0 = r6;				// i0 -> first input buffer 'inx'
//	b0 = r6;				// b0 -> base of first input circular buffer
/******************************/

/*********IPDC addition*******/
	p5 = 44;
	r6 = [p0++p5];			// r6 -> first input buffer 'inx'
	i0 = r6;				// i0 -> first input buffer 'inx'
	p5=-40;
	r6 = [p0++p5];
	b0 = r6;				// b0 -> base of first input circular buffer
/******************************/
	r6 = [p0++];
	r6 = r6 << 2;			// double length (4 bytes per word)
	l0 = r6;				// l0 = first input circular buffer size 'SZINx'

go_back:

//	p3.l = num_blocks;
//	p3.h = num_blocks;	
//	r6 = w[p3];				// get number of blocks
//	r6 += -1;				// Decrement number of blocks	
//	w[p3] = r6;				// save decremented number of blocks
//	CC = r6 < 0;			
//	IF CC JUMP RETURN_TO_SENDER;				// Return if less than 1 block

    p3.l = inputs;		// p3 -> to number of input samples per block
	p3.h = inputs;
	r7 = w[p3];
	p5 = r7;			// p5 =  number of input samples per block

	i1 = p1;			// load i1 with address of 'inputData'
	l1 = 0;


	LSETUP(READ_INPUTS_BEGIN, READ_INPUTS_END) LC0 = p5;
READ_INPUTS_BEGIN:
		r6.h = w[i1++];			// read the input buffer 'inputData'
		r6.l = 0;

READ_INPUTS_END:
		[i0++] = r6;			// write input into input buffer 'inx'

//	p1 = i1; 			// save i1 into p1

    p3.l = pt_fundst;		// p3 -> to fundemental structure
	p3.h = pt_fundst;
	r7 = [p3];
	p0 = r7;				// p0 -> to fundemental structure

#endif

src_core:
    
	r7 = [p0++];			
	p3.l = st_handle;
	p3.h = st_handle;
	[p3] = r7;			// store stage data handle

	r6 = [p0++];		// r6 = half band flag (move past this for now)

	r2 = [p0++];		// r2 = # of up stages
	p3.l = pt2_fundst;
	p3.h = pt2_fundst;
	[p3] = p0;			// save pointer to current fundemental structure


	CC = r2 <= 0;
	IF CC JUMP over_upstage;		// if upstage = 0, jump over

UPSTAGE_BEGIN:

		p3.l = st_handle;
		p3.h = st_handle;
		p4 = [p3]; 			// p4 -> current stage data handle

		r7 = [p4++];		
		p0 = r7;			// p0 -> stage data

		[p3] = p4;			// save pointer to stage data handle

up_src:
/******** IPDC comment	 *******/
//		r7 = [p0++];			// r7 -> input signal 'inx'
//		b3 = r7;				// b3 set for circular buffering
/******************************/

/*********IPDC addition*******/
		p4 = 44;
		r7 = [p0++p4];			// r7 -> input signal 'inx'
		p4 = -40;
		r5 = [p0++p4];
		b3 = r5;				// b3 set for circular buffering
/******************************/

		r5 = [p0++];
		r5 = r5 << 2;			// double the length (4 bytes per word)		
		l3 = r5;				// l3 = Size of Input Stage (SIS)

/******** IPDC comment	 *******/
//		r6 = [p0++];
//		i2 = r6;				// i2 -> output signal 'inx'+1 buffer (output buffer)
//		b2 = r6;				// b2 set for circular buffering

/******************************/

/*********IPDC addition*******/
				
		p4 = 40;
		r6 = [p0++p4];
		i2 = r6;				// i2 -> output signal 'inx'+1 buffer (output buffer)
		p4 = -36;
		r6 = [p0++p4];
		b2 = r6;				// b2 set for circular buffering
/******************************/

		r6 = [p0++];
		r6 = r6 << 2;			// double the output size (4 bytes per word)
		l2 = r6;				// l2 = Size of Output Stage (SOS)

		r3 = [p0++];			// r3 -> the filter coefficients
		r6 = [p0++];			// r6 = poly-phase filter size
		p3 = r6;				// save poly-phase into p3

		p4 = 8;					// always skip over DNR (2*4bytes) in the up SRC

		r4 = [p0++p4];			// r4 = UPR
		p5 = r4;				// p5 = Up Sample Rate (UPR)
		r0 = [p0++p4];			// r0 = NIS

		p4 = -40;				// Backup 10 words (10x4bytes)
		r6 = [p0++p4];			// r6 = number of shifts (always a arithmatic left shift..upshift)
		m2 = r6;				// Save in m2

UP_SRC_OUTER_BEGIN:

			i1 = r3;				// i1 -> filter coefficients
			l1 = 0;					// linear addressing???
	
			LSETUP(UP_SAMPLE_BEGIN, UP_SAMPLE_END) LC0 = p5;

UP_SAMPLE_BEGIN:
				i3 = r7;			// i3 - > 'in' buffer		
				A1=A0=0 || R6=[I1++] || R5=[I3--]; // r6=filter coef, r5='inx' buffer

				LSETUP(POLY_PHASE_BEGIN, POLY_PHASE_END) LC1 = p3;

POLY_PHASE_BEGIN:	R4=(A0+=R6.H*R5.H), A1+=R6.H*R5.L (M);			
POLY_PHASE_END:		R1=(A1+=R5.H*R6.L) (M) || R6=[I1++] || R5=[I3--];
				
//				R1=R1>>16;
//				R4=R4+R1 (S);

				r5=m2;					// load r5 with number of shifts
/******** IPDC comment	 *******/
//				A1 = A1>>16;
/******************************/

/*********IPDC addition*******/
				A1=A1>>>15;
/******************************/
				A0+=A1;
				A0 = ASHIFT A0 BY r5.l;
				r4 = A0;			    // high half-word extraction with 16-bit saturation.  Rounding cntrl by 
										// RND_MOD.  0 = unbiased rounding = default




//				A0 = A0 >>> 1;
//				R4 = A0;


UP_SAMPLE_END:	[i2++] = R4;			// save output into 'inx'+1

			i3 = r7;			// get input back at beginning of 'inx'
			m3 = 4;
			i3 += m3;			// increment by 1 word (4 bytes)
			r7 = i3;			// update r7 -> 'inx' buffer

UP_SRC_OUTER_END:
		r0 += -1;						// Check number of input samples (NIS)
		CC = r0 <= 0;
		IF !CC JUMP UP_SRC_OUTER_BEGIN;	// if NIS equal to 0, jump to UP_SRC_OUTER_BEGIN

		p4 = 8;							// 2 words (2*4bytes per word)
		[p0++p4] = r7;					// save the input signal address

		r6 = i2;
		[p0] = r6;						// save the output signal address
	
UPSTAGE_END:
	r2 += -1;						// Check number of stages
	CC = r2 <= 0;
	IF !CC JUMP UPSTAGE_BEGIN;		// if upstage not equal to 0, jump to UPSTAGE_BEGIN

over_upstage:

	p3.l = pt2_fundst;
	p3.h = pt2_fundst;
	p0 = [p3];			// p0 -> fundamental structure

	r6 = [p0++];		// r6 = pivot flag
	[p3] = p0;			// save fundamental structure

	CC = r6 <= 0;
	IF CC JUMP over_pivotstage;		// if pivotstage = 0, jump over

	p3.l = st_handle;
	p3.h = st_handle;
	p4 = [p3]; 			// p4 -> current stage data handle

	r7 = [p4++];		
	p0 = r7;			// p0 -> stage data