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

_mat_mat4:

	    ldi 4, IR0		    ;do this first to avoid pipeline conflict
	    ldi 3, IR1
;
;   first, set up the addresses
;
	    ldi ST, R0		    ;save ST
	    ldi 3, RC		    ;set repeat counter
	    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

	    rptb    vector4

	    mpyf3   *AR0++, *AR1++(IR0), R0
	    mpyf3   *AR0++, *AR1++(IR0), R1

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

	    nop     *AR1--(11)

	    mpyf3   *AR0++, *AR1++(IR0), R0
	||  addf3   R0, R3, R3


	    mpyf3   *AR0++, *AR1++(IR0), R1

	    mpyf3   *AR0++, *AR1++(IR0), R0
	||  addf3   R0, R1, R2


	    mpyf3   *AR0--(IR1), *AR1, R0
	||  addf3   R0, R2, R2

	    nop     *AR1--(11)

	    mpyf3   *AR0++, *AR1++(IR0), R1
	||  addf3   R0, R2, R2

	    stf     R3, *AR2
	||  stf     R2, *++AR2

	    nop     *AR2++

	    mpyf3   *AR0++, *AR1++(IR0), R0

	    mpyf3   *AR0++, *AR1++(IR0), R0
	||  addf3   R0, R1, R2

	    mpyf3   *AR0--(IR1), *AR1, R0
	||  addf3   R0, R2, R2

	    nop     *AR1--(11)

	    mpyf3   *AR0++, *AR1++(IR0), R0
	||  addf3   R0, R2, R2

	    mpyf3   *AR0++, *AR1++(IR0), R1

	    mpyf3   *AR0++, *AR1++(IR0), R0
	||  addf3   R0, R1, R3

	    mpyf3   *AR0++, *AR1, R0
	||  addf3   R0, R3, R3

	    addf3   R0, R3, R3

	    stf     R2, *AR2
	||  stf     R3, *++AR2

	    nop     *AR1--(15)

vector4:    nop     *AR2++


	    rets			    ;all done

	    .end