;
;   calculates c = a x b
;
;   float   a[4][4]
;   float   b[4]
;   float   c[4]
;
;   void    mat_vec4( c, a, b )
;
;
;      James M. Patterson
;      Texas Instruments
;      370 South North Lake Boulevard
;      Altamonte Springs, Florida 32701
;
;
FP	    .set    AR3
	    .global _mat_vec4
	    .text

_mat_vec4:

	    ldi 3, IR0		    ;do this first to avoid pipeline

;
;   first, set up the addresses
;
	    ldi ST, R0		    ;save ST
	    or	~2000h, ST	    ;clear GIE
	    pop AR2		    ;do this to back up SP
	    pop AR2		    ;AR2 points to C vector
	    pop AR0		    ;AR0 points to A matrix
	    pop AR1		    ;AR1 points to B vector
	    addi    4, SP	    ;restore the SP
	    ldi R0, ST		    ;restore ST register


;
;   calculate c[0]
;

	    mpyf3   *AR0++, *AR1++, R0		;a[0][0] * b[0] -> R0
	    mpyf3   *AR0++, *AR1++, R1		;a[0][1] * b[1] -> R1


	    mpyf3   *AR0++, *AR1++, R0
	||  addf3   R0, R1, R3		    ;R0 + R1 -> R3
					    ;a[0][2] * b[2] -> R0
	    mpyf3   *AR0++, *AR1--(IR0), R1
	||  addf3   R0, R3, R3		    ;R0 + R3 -> R3
					    ;a[0][3] * b[3] -> R1


	    mpyf3   *AR0++, *AR1++, R0	    ;a[1][0] * b[0] -> R0
	||  addf3   R0, R3, R3		    ;R0 + R3 ->R3
					    ;this completes c[0] in R3


	    mpyf3   *AR0++, *AR1++, R1	    ;a[1][1] * b[1] ->R1

	    mpyf3   *AR0++, *AR1++, R0
	||  addf3   R0, R1, R2		    ;R0 + R1 -> R2
					    ;a[1][2] * b[2] -> R0

	    mpyf3   *AR0++, *AR1--(IR0), R0 ;a[1][3] * b[3] -> R0
	||  addf3   R0, R2, R2		    ;R0 + R2 -> R2

	    mpyf3   *AR0++, *AR1++, R1	    ;a[2][0] * b[0] ->R1
	||  addf3   R0, R2, R2		    ;finish c[1] in R2

	    stf     R3, *AR2
	||  stf     R2, *++AR2		    ;save c[0] and c[1]

	    nop     *AR2++		    ;bump c address pointer

	    mpyf3   *AR0++, *AR1++, R0	    ;a[2][1] * b[1] -> R0

	    mpyf3   *AR0++, *AR1++, R0	    ;a[2][2] * b[2] -> R0
	||  addf3   R0, R1, R2		    ;R0 + R1 -> R2

	    mpyf3   *AR0++, *AR1--(IR0), R0 ;a[2][3] * b[3] -> R0
	||  addf3   R0, R2, R2		    ;R0 + R2 -> R2

	    mpyf3   *AR0++, *AR1++, R0	    ;a[3][0] * b[0] -> R0
	||  addf3   R0, R2, R2		    ;R2 now has c[2]

	    mpyf3   *AR0++, *AR1++, R1	    ;a[3][1] * b[1] -> R1

	    mpyf3   *AR0++, *AR1++, R0	    ;a[3][2] * b[2] -> R0
	||  addf3   R0, R1, R3		    ;R0 + R1 -> R3

	    mpyf3   *AR0, *AR1, R0	    ;a[3][3] * b[3] -> R0
	||  addf3   R0, R3, R3		    ;R0 + R3 -> R3

	    addf3   R0, R3, R3		    ;R3 now has c[3]


	    stf     R2, *AR2		    ;save c[2]
	||  stf     R3, *++AR2		    ;save c[3]


	    rets			    ;all done

	    .end