martix math

;Matrix Multiplier 3x3 by 3x3
;Written by: Marcel Verdaasdonk

;registers used
;fp0 R0
;fp1 R1
;fp2 R2
;fp3 C0
;fp4 C1
;fp5 C2
;A0 Base Matrix 0
;A3 Index Matrix 0
;A1 Base Matrix 1
;A4 Index Matrix 1
;A2 Base Matrix Sigma
;A5 Index Matrix Sigma
;d0 loop counter
;d1 offset
;d2 outer loop counter

    fmovem.x fp5-fp0, -(a7)
    movem.l a5-a3, -(a7)
    movea.l a1, a4
    movea.l a0, a3
    movea.l a2, a5
    moveq #2, d1
OUT:
    fmove.s (a4)+, FP3
    addq.l #8, a4
    moveq #2, d0
    fmove.s (a4)+, fp4
    addq.l #8, a4
    fmove.s (a4), fp5
LOOP:
    fmove.s (a3)+, fp0
    fsmul fp3, fp0
    fmove.s (a3)+, fp1
    fsmul fp4, fp1
    fmove.s (a3)+, fp2
    fsmul fp5, fp2
    fadd.s fp0, fp1
    fadd.s fp1, fp2
    fmove.s fp2, (a5)+
    addq.l #8, a5
    dbra.s d0, LOOP
    movea.l a1, a4
    addq.l #4, a4
    movea.l a0, a3
    addq.l #4, a3
    movea.l a2, a5
    addq.l #4, a5
    moveq #8, d1
    dbra.s d1, OUT
    movem.l (a7)+, a5-a3
    fmovem.x (a7)+, fp5-fp0
    rts