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

_mat_vec3:

	    ldi 2, IR0		    ;do this first to avoid pipeline

;
;   first, set up the addresses
;
	    ldi ST, R0		    ;save status
	    and ~2000h, ST	    ;clear GIE
				    ;don't want ints while playing with stack
	    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 status

;
;   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--(IR0), R0
	||  addf3   R0, R1, R3		    ;R0 + R1 -> R3
					    ;a[0][2] * b[2] -> R0
;
;   this completes calculation of c[0]
;   and begins c[1]
;
	    mpyf3   *AR0++, *AR1++, R1
	||  addf3   R0, R3, R3		    ;R0 + R3 -> R3
					    ;a[1][0] * b[0] -> R1
;
;   continue calculating c[1]
;


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

	    mpyf3   *AR0++, *AR1--(IR0), R1
	||  addf3   R0, R1, R2		    ;R0 + R1 -> R3
					    ;a[1][2] * b[2] ->R1
;
;   finish with c[1]
;   begin c[2]
;
	    mpyf3   *AR0++, *AR1++, R0
	||  addf3   R1, R2, R2		    ;R1 + R3 -> R3
					    ;a[2][0] * b[0] -> R0

;
;   store c[0], c[1]
;   continue with c[2]
;

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

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

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

	    addf    R0, R3		    ;complete summation of c[2]
	    stf     R3, *++AR2		    ;save c[2]

	    rets			    ;all done
					    ;delayed rets would be nice her

	    .end