//----------------------------------------------------------------------------------
// <copyright file="Math.S" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	Assembly language integer math routines
// </summary>
//
// <owner>Jim Kirklin</owner>
//---------------------------------------------------------------------------------

/*******************************************************
               Multiplication  32 x 32
*******************************************************/
#define r_cpy1L  r18
#define r_cpy1H  r19
#define r_arg1L  r24		/* multiplier Low */
#define r_arg1H  r25

#define	r_arg2L  r22		/* multiplicand Low */
#define	r_arg2H  r23	
	
#define r_resL	 r26		/* result Low */
#define r_resH   r27
#define r_resHL	 r30
#define r_resHH  r31		/* result High */

	
	.global	umult16x16
	.func	umult16x16
umult16x16:
	mul	    r_arg1L, r_arg2L
	movw	r_resL, r0
	mul	    r_arg1H, r_arg2H
	movw	r_resHL, r0
	mul	    r_arg1H, r_arg2L
    clr     r_arg2L
	add	    r_resH, r0
	adc	    r_resHL, r1
    adc     r_resHH, r_arg2L
	mul	    r_arg1L, r_arg2H
	add	    r_resH, r0
	adc	    r_resHL, r1
    adc     r_resHH, r_arg2L

; put the result back
	movw	r_arg2L, r_resL
	movw	r_arg1L, r_resHL
	clr	r1		; __zero_reg__ clobbered by "mul"
	ret
    .endfunc

	.global	smult16x16
	.func	smult16x16
smult16x16:
    movw    r_cpy1L, r_arg1L
	mul	    r_arg1L, r_arg2L
	movw	r_resL, r0
	muls    r_cpy1H, r_arg2H
	movw	r_resHL, r0
	mulsu   r_cpy1H, r_arg2L
    clr     r_arg2L
	add	    r_resH, r0
	adc	    r_resHL, r1
    adc     r_resHH, r_arg2L
	mulsu	r_arg2H, r_cpy1L
	add	    r_resH, r0
	adc	    r_resHL, r1
    adc     r_resHH, r_arg2L

; put the result back
	movw	r_arg2L, r_resL
	movw	r_arg1L, r_resHL
	clr	r1		; __zero_reg__ clobbered by "mul"
	ret
    .endfunc

    .end