#include "idct.h"

/*
    Inputs:

	idct_cbp	- Coded Block Pattern (bit N is a flag for IDCTing that block
	idct_in_dat	- Input Data address
	idct_out_dat	- Output Data Address (can be the same as the in_dat)
    
    Outputs:

	data at idct_out_dat - IDCT'ed Input Data
*/

idct:
	nop
	nop
	nop

idct_init:	
	addi caddr, rzero, IDCT_CONST_BASE
	lqv a00, AMAT0L(caddr)
	lqv a08, AMAT0H(caddr)
	lqv a10, AMAT1L(caddr)
	lqv a18, AMAT1H(caddr)

	lqv consts, IDCT_CONSTS(caddr)

	nop
	nop

	vadd vone, vzero, consts[ONE]
	

idct_loop:
	nop
	nop
	nop

	andi	dum, idct_in_cbp, 1
	bgtz	dum, idct_do
	srl	idct_in_cbp, idct_in_cbp, 1

idct_ret:	
	blez	idct_in_cbp, idct_done
	nop

	addi	idct_in_dat, idct_in_dat, 128
	addi	idct_out_dat, idct_out_dat, 128

	j	idct_loop
	nop

idct_done:
	jr	return
	nop
	nop

idct_do:
	nop
	nop

	lqv x20, ROW0(idct_in_dat)
	lqv x21, ROW4(idct_in_dat)
	lqv x22, ROW2(idct_in_dat)
	lqv x23, ROW6(idct_in_dat)

	lqv x24, ROW1(idct_in_dat)
	lqv x25, ROW5(idct_in_dat)
	lqv x26, ROW3(idct_in_dat)
	lqv x27, ROW7(idct_in_dat)

	nop
	nop
	nop
	nop
	nop
	nop

	vmudh p0, x20, consts[SCALEUP]
	vmudh p1, x21, consts[SCALEUP]
	vmudh p2, x22, consts[SCALEUP]
	vmudh p3, x23, consts[SCALEUP]

	vmudh q0, x24, consts[SCALEUP]
	vmudh q1, x25, consts[SCALEUP]
	vmudh q2, x26, consts[SCALEUP]
	vmudh q3, x27, consts[SCALEUP]

	vmulf vdum, p0, a00[0]
	vmacf vdum, p1, a00[1]
	vmacf vdum, p2, a00[2]
	vmacf x00, p3, a00[3]

	vmulf vdum, p0, a00[4]
	vmacf vdum, p1, a00[5]
	vmacf vdum, p2, a00[6]
	vmacf x01, p3, a00[7]

	vmulf vdum, p0, a08[0]
	vmacf vdum, p1, a08[1]
	vmacf vdum, p2, a08[2]
	vmacf x02, p3, a08[3]
	
	vmulf vdum, p0, a08[4]
	vmacf vdum, p1, a08[5]
	vmacf vdum, p2, a08[6]
	vmacf x03, p3, a08[7]


	vmulf vdum, q0, a10[0]		/* 2nd Matrix */
	vmacf vdum, q1, a10[1]
	vmacf vdum, q2, a10[2]
	vmacf x10, q3, a10[3]

	vmulf vdum, q0, a10[4]
	vmacf vdum, q1, a10[5]
	vmacf vdum, q2, a10[6]
	vmacf x11, q3, a10[7]

	vmulf vdum, q0, a18[0]
	vmacf vdum, q1, a18[1]
	vmacf vdum, q2, a18[2]
	vmacf x12, q3, a18[3]

	vmulf vdum, q0, a18[4]
	vmacf vdum, q1, a18[5]
	vmacf vdum, q2, a18[6]
	vmacf x13, q3, a18[7]

	nop
	nop
	nop
	nop

	vadd x20, x00, x13		/* Butterflies with no rounding */
	vadd x21, x01, x12
	vadd x22, x02, x11
	vadd x23, x03, x10
	vsub x24, x03, x10
	vsub x25, x02, x11
	vsub x26, x01, x12
	vsub x27, x00, x13


	vmudm	vdum, x20, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x20, $v0, consts[ONE]
	
	vmudm	vdum, x21, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x21, $v0, consts[ONE]
	
	vmudm	vdum, x22, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x22, $v0, consts[ONE]
	
	vmudm	vdum, x23, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x23, $v0, consts[ONE]
	
	vmudm	vdum, x24, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x24, $v0, consts[ONE]
	
	vmudm	vdum, x25, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x25, $v0, consts[ONE]
	
	vmudm	vdum, x26, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x26, $v0, consts[ONE]
		
	vmudm	vdum, x27, consts[HALF]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x27, $v0, consts[ONE]

			
	/* Transpose */

	addi	caddr, rzero, TEMP_BASE
	nop
	nop
	nop
	nop
	nop
	nop

	stv	x20[0], ROW0(caddr)
	stv	x20[2], ROW1(caddr)
	stv	x20[4], ROW2(caddr)
	stv	x20[6], ROW3(caddr)
	stv	x20[8], ROW4(caddr)
	stv	x20[10], ROW5(caddr)
	stv	x20[12], ROW6(caddr)
	stv	x20[14], ROW7(caddr)
	
	nop
	nop
	nop
	nop
	nop
	nop

	ltv	p0[0], ROW0(caddr)
	ltv	p0[14], ROW1(caddr)
	ltv	p0[12], ROW2(caddr)
	ltv	p0[10], ROW3(caddr)
	ltv	p0[8], ROW4(caddr)
	ltv	p0[6], ROW5(caddr)
	ltv	p0[4], ROW6(caddr)
	ltv	p0[2], ROW7(caddr)

		/* 2nd Pass */

	nop
	nop
	nop
	nop
	nop
	nop


	vmulf vdum, p0, a00[0]
	vmacf vdum, p1, a00[1]
	vmacf vdum, p2, a00[2]
	vmacf x00, p3, a00[3]

	vmulf vdum, p0, a00[4]
	vmacf vdum, p1, a00[5]
	vmacf vdum, p2, a00[6]
	vmacf x01, p3, a00[7]

	vmulf vdum, p0, a08[0]
	vmacf vdum, p1, a08[1]
	vmacf vdum, p2, a08[2]
	vmacf x02, p3, a08[3]

	vmulf vdum, p0, a08[4]
	vmacf vdum, p1, a08[5]
	vmacf vdum, p2, a08[6]
	vmacf x03, p3, a08[7]

	vmulf vdum, q0, a10[0]		/* 2nd Matrix */
	vmacf vdum, q1, a10[1]
	vmacf vdum, q2, a10[2]
	vmacf x10, q3, a10[3]

	vmulf vdum, q0, a10[4]
	vmacf vdum, q1, a10[5]
	vmacf vdum, q2, a10[6]
	vmacf x11, q3, a10[7]

	vmulf vdum, q0, a18[0]
	vmacf vdum, q1, a18[1]
	vmacf vdum, q2, a18[2]
	vmacf x12, q3, a18[3]

	vmulf vdum, q0, a18[4]
	vmacf vdum, q1, a18[5]
	vmacf vdum, q2, a18[6]
	vmacf x13, q3, a18[7]

	nop
	nop
	nop
	nop

	vadd x20, x00, x13		/* Butterflies with no rounding */
	vadd x21, x01, x12
	vadd x22, x02, x11
	vadd x23, x03, x10
	vsub x24, x03, x10
	vsub x25, x02, x11
	vsub x26, x01, x12
	vsub x27, x00, x13

	nop
	nop
	nop
	nop

/*

	Final Rounding and Scaling
*/

#define bit0	x00
#define bit1	x01
#define bit2	x02
#define bit3	x03

#define bit4	x10
#define bit5	x11
#define bit6	x12
#define bit7	x13

	/*  The x2's now have 6 bits of fraction that needs to be
	    properley rounded.  Symmetric rounding is very important
	    here since a bias will affect about 1% (1/128) of the
	    values.

	    The following gives symmetric rounding:

	    if( x2 < 0 )
		result = (x2 + 0x1f) >> 6;
	    else
		result = (x2 + 0x20) >> 6;
	    
	    I have implemented this via:

	    accum = 0
	    accum += x2*(-1)
	    bit   = accum>>15;	(-1 if x2 > 0, else 0 )

	    accum = (x2 + 0x20) * (1<<(15-6));
	    accum += bit
	    result = accum >> 15;
	*/

	/* Sets bit0 to -1 if x20>0 */
/*	
	vmudh bit0, x20, consts[ZERO]
	vmacf bit0, x20, consts[NEG1]	
	vmudh bit1, x21, consts[ZERO]
	vmacf bit1, x21, consts[NEG1]
	vmudh bit2, x22, consts[ZERO]
	vmacf bit2, x22, consts[NEG1]
	vmudh bit3, x23, consts[ZERO]
	vmacf bit3, x23, consts[NEG1]
	vmudh bit4, x24, consts[ZERO]
	vmacf bit4, x24, consts[NEG1]
	vmudh bit5, x25, consts[ZERO]
	vmacf bit5, x25, consts[NEG1]
	vmudh bit6, x26, consts[ZERO]
	vmacf bit6, x26, consts[NEG1]
	vmudh bit7, x27, consts[ZERO]
	vmacf bit7, x27, consts[NEG1]
*/
	/* Decriments x20>>6 if x20>0 */
/*	
	vmulf vdum,  x20, consts[SCALEDN]
	vmacf x20, bit0, consts[ONE]	
	vmulf vdum,  x21, consts[SCALEDN]
	vmacf x21, bit1, consts[ONE]
	vmulf vdum,  x22, consts[SCALEDN]
	vmacf x22, bit2, consts[ONE]
	vmulf vdum,  x23, consts[SCALEDN]
	vmacf x23, bit3, consts[ONE]
	vmulf vdum,  x24, consts[SCALEDN]
	vmacf x24, bit4, consts[ONE]
	vmulf vdum,  x25, consts[SCALEDN]
	vmacf x25, bit5, consts[ONE]
	vmulf vdum,  x26, consts[SCALEDN]
	vmacf x26, bit6, consts[ONE]
	vmulf vdum,  x27, consts[SCALEDN]
	vmacf x27, bit7, consts[ONE]
*/
	
	vmudm	vdum, x20, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x20, $v0, consts[ONE]
	
	vmudm	vdum, x21, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x21, $v0, consts[ONE]
	
	vmudm	vdum, x22, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x22, $v0, consts[ONE]
	
	vmudm	vdum, x23, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x23, $v0, consts[ONE]
	
	vmudm	vdum, x24, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x24, $v0, consts[ONE]
	
	vmudm	vdum, x25, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x25, $v0, consts[ONE]
	
	vmudm	vdum, x26, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x26, $v0, consts[ONE]
		
	vmudm	vdum, x27, consts[SHIFT5]
	vmadm	vdum, vone, consts[SUBHALF]
	vrndn	x27, $v0, consts[ONE]
	
	
			
		/* Transpose */
	nop
	nop
	nop
	nop
	nop
	nop

	
	stv	x20[0], ROW0(caddr)
	stv	x20[2], ROW1(caddr)
	stv	x20[4], ROW2(caddr)
	stv	x20[6], ROW3(caddr)
	stv	x20[8], ROW4(caddr)
	stv	x20[10], ROW5(caddr)
	stv	x20[12], ROW6(caddr)
	stv	x20[14], ROW7(caddr)
	
	nop
	nop
	nop
	nop
	nop
	nop

	ltv	p0[0], ROW0(caddr)
	ltv	p0[14], ROW1(caddr)
	ltv	p0[12], ROW2(caddr)
	ltv	p0[10], ROW3(caddr)
	ltv	p0[8], ROW4(caddr)
	ltv	p0[6], ROW5(caddr)
	ltv	p0[4], ROW6(caddr)
	ltv	p0[2], ROW7(caddr)

	nop
	nop
	nop
	nop
	nop
	nop

	sqv	p0[0], ROW0(idct_out_dat)
	sqv	q0[0], ROW1(idct_out_dat)
	sqv	p2[0], ROW2(idct_out_dat)
	sqv	q2[0], ROW3(idct_out_dat)
	sqv	p1[0], ROW4(idct_out_dat)
	sqv	q1[0], ROW5(idct_out_dat)
	sqv	p3[0], ROW6(idct_out_dat)
	sqv	q3[0], ROW7(idct_out_dat)

		/* Save it back or blend in reference */
	
	nop
	nop
	nop
	nop
	nop

	j	idct_ret
	nop
	nop

#include "idct_un.h"