/*******************************************************************************
Module Name     : YUV422toRGB565.asm
Label Name      : __YUV422toRGB565
Version         :   1.0

Author: Roland Oberhammer

Description     : In this function the range of Y, U and V is 0 to 255.


Prototype       : void YUV422toRGB565(unsigned char input[], unsigned char out[], 
                                  int N);

                     input[] - Input YCbCr array 
                       out[] - Out put  array to store in RGB format
                           N - Number of inputs (xsize  * ysize)

Registers used  : A0, A1, R0-R7, I1, B1, L1, P0-P2, LC0.

*******************************************************************************/
.section    L1_code;
.global     __YUV422toRGB565;
.align      8;
    
__YUV422toRGB565:   
	[--SP] = (R7:0, P5:0);  // pushing the Registers on stack. 
    SP += -8;               // SP modified to store coefficients
    I1 = SP;                //
    B1 = I1;                // initialize base register B1 and I1  for circular buffer
    L1 = 8;                 // Initialize length for circular buffer
    
    P0 = R0;                // address of input YUV array in P0
    P2 = R1;                // address of output array to store RGB565 values in P2
    R2 >>= 1;		        // Number of inputs N / 2 (N must be XSIZE * YSIZE)
    P1 = R2;
    
    R6.L = 0x6c28;			// 169 / 400 * 65536			
    R6.H = 0x2147;			// 52 / 400 * 65536          
    R7.L = 0xC29;			// 19 / 400 * 65536
    R7.H = 0;				// coefficients 19 and 400  are stored in R7
    [I1++] = R6;			// store coefficients 52 and 169 to temp. location
        
    R5 = R7-R7(NS) || [I1++] = R7;
    						// R5 = 0 (for minimum comparing)
                            // store coefficients 19 in temp. location

    R0 = B[P0++](Z);         // fetch first U value in R0       
    LSETUP(RGB565_LOOP_START, RGB565_LOOP_END) LC0 = P1;
                            
RGB565_LOOP_START:                                      
    R7 = 128;				 // initialize R7 to 128
	R4 = 255;				 // initialize R4 to 255
    R0 = R0 - R7(NS) || R1 = B[P0++](Z);
    						// U(R0) -= 128 and fetch first y value in R1
    R6 = R0 + R1(NS) || R2 = B[P0++](Z);
    						// calculate first blue value in R6 and fetch v value in R2
    R2 = R2 - R7(NS);		// V(R2) -= 128
    R6 = MAX(R6,R5)(V);		// limit blue
    R6 = MIN(R6,R4)(V);
    R3 = R2 + R1;			// calculate first red value
    R3 = MAX(R3,R5)(V);		// control upper limit of red
    R3 = MIN(R3,R4)(V) || R7 = [I1++];
    						// control lower limit of red and fetch first coefficients
    //R1 <<= 16;
	//R3 = R3 | R1;			// R3.H = y, R3.L = red
	R3 = PACK(R1.L, R3.L);
	    						
    A0 = R7.L * R3.H, A1 = R7.H * R3.L || R7 = [I1++];
    						// A0 = 169/400 * y, A1 = 52/400 * red and fetch second coefficients
	A0 -= A1;
	R4 = (A0 -= R7.L * R6.L) || R1 = B[P0++](Z);
							// first green = A0 = A0 - 19/400 * blue in r4
							// and fetch second Y value in R1
	
	R4 = R4 >> 17;			// green = green / 65535 << 5
	R7 = 63;
    R4 = MAX(R4,R5)(V);		// limit green
    R4 = MIN(R4,R7)(V);
	R4 = R4 << 5;
	
	
	
	R7.L = 0xf800;
	R6 = R6 << 8;			// blue = blue / 8(>>3) << 11
	R3.H = 0;				
	R6 = R6 & R7;			// mask the blue value
	R3 >>= 3;				// red = red / 8(>>3);
	R6 = R6 | R3;			// combine blue and red
	R6 = R6 | R4;			// add green to rgb565
	R4 = 255;				// initialize R4 to 255
	R3 = R0 + R1(NS) || W[P2++] = R6;
							// calculate second blue value in R3 and store first rgb565 in output array
    R3 = MAX(R3,R5)(V);		// limit blue
    R3 = MIN(R3,R4)(V);
	R6 = R2 + R1(NS);		// calculate second red value in r6
    R6 = MAX(R6,R5)(V);		// control upper limit of red
    R6 = MIN(R6,R4)(V) || R7 = [I1++];
    						// control lower limit of red and fetch first coefficients
	//R1 <<= 16;
	//R6 = R6 | R1;			// R6.H = y, R6.L = red
	R6 = PACK(R1.L, R6.L);
	    						
    A0 = R7.L * R6.H, A1 = R7.H * R6.L || R7 = [I1++];
    						// A0 = 169/400 * y, A1 = 52/400 * red and fetch second coefficients
	A0 -= A1;
	R4 = (A0 -= R7.L * R3.L) || R0 = B[P0++](Z);
							// second green = A0 = A0 - 19/400 * blue in r4
							// and fetch first U in R0
	
	
	R4 = R4 >> 17;			// green = green / 65535 << 5
	R7 = 63;
    R4 = MAX(R4,R5)(V);		// limit green
    R4 = MIN(R4,R7)(V);
	R4 = R4 << 5;

	
					
	R7.L = 0xf800;
	R3 = R3 << 8;			// blue = blue / 8(>>3) << 11
	R6.H = 0;				
	R3 = R3 & R7;			// mask the blue value
	R6 >>= 3;				// red = red / 8(>>3);
	R3 = R3 | R6;			// combine blue and red
	R3 = R3 | R4;			// add green to rgb565
RGB565_LOOP_END:    
    W[P2++] = R3;			// store second rgb565 value in output array
    
    SP += 8;                // Clear temp. location
    (R7:0, P5:0) = [SP++];        // Pop up the saved registers.
    RTS; 
    NOP;                    //to avoid one stall if LINK or UNLINK happens to be
                            //the next instruction after RTS in the memory.
__YUV422toRGB565.end: