* Misc.s functions we use

********************************************************
* ** NAME: memcpy - copies NIBBLES
*
* Purpose:
*
* Entry: D0   - src
*        D1   - dest
*        A[A] - number of NIBBLES to copy
*
* Exit:  
*             
*
* Alters: A[A],C[B],C[S],D0,D1 
*
* Calls:
*
* Stack Levels: 0 
*
*
********************************************************
memcpy
	?ABIT=0	0				* is even?
	GOYES	+
	C=DAT0	S
	DAT1=C	S				* copy one nibble
	D0=D0+	1				* move pointers
	D1=D1+	1
+	ASRB.F	A				* divide by two
	GOTO	+

-	
	C=DAT0	B				* read  2 nibbles
	DAT1=C	B				* write 2 nibbles
	D0=D0+	2				* increment
	D1=D1+	2				* increment
+	A=A-1	A
	GONC	-
	RTN



********************************************************
* ** NAME: MulAdd - computes (A*B+C*D)[B] in 1.7 signed fixed point
*
* Category:
*
* Purpose:
*
* Entry: A,B,C,D [B] are values to work with
*
* Exit: C[14-15] is result in 1.7 signed fixed point (nibbles 14,15 are the new value)
*
* Alters: A,B,C,D, leaving [B] parts fixed, modifies ST bit 3
*
* Calls: 
*
* Stack Levels: 0
*
* Notes:
*    todo - speeding this up will result in a big speed improvement
*						
*  Date     Prog	 Modification
* ------  --------  --------------
*
********************************************************

* macro multiplies A[B]*B[B]->C[B] in 1.7 signed fixed point
MulABC MACRO
	C=0			A
	LC(2)		#FF
	A=A&C		A
	B=B&C		A			* mask these out
	ST=0		3			* signals A is positive
	?ABIT=0		7			* is positive?
	GOYES		+
	ST=1		3			* signals A is negative
	A=-A		B			* negate - speeds up mpn later
+	ABEX		A			* A and B  swapped here - so loop on B below
	?ABIT=0		7
	GOYES		+
	C=-C-1		A			* flip all bits
	A=A!C		A			* extend negative bits....
+	
	C=0			A			* zero this out, we will sum into here
	SB=0					* check bits set with this
-	BSRB.F		A 			* we mult in a loop
	?SB=0 
	GOYES		+
	SB=0 
	C=C+A		A 
+   A=A+A		A 
	?B#0		A 
	GOYES		-

	C=C+C		A			* now scale back 7 bits
	CSR			A
	CSR			X			* C[B] is now result (shift [X] faster than [A])

	?ST=0		3			* do we need to negate?
	GOYES		+
	C=-C		B			* negate it!
+

	ENDM					* MulABC


MulAdd
	DSL			X
	DSL			A
	D=C			B			* D low is now original (D,C)[B]

	C=B			B
	CSRC		
	CSRC
	C=A			B
	CSRC		
	CSRC					* C high is now (A,B)[B] original

* mult, getting A*B into C[B], uses A,B,C[A]
	MulABC

	CDEX		A
	A=C			A			* get D,C into A
	CDEX		A
	DSL			W			* make space....
	DSL			W
	D=C			B			* store, now D holds (D,C,A*B)[B] from originals
	B=A			B			* original C value here
	ASR			A			* getting D here to prepare C*D
	ASR			X			* faster than [A]

* mult, getting C*D into C[B], uses A,B,C[A]
	MulABC

	C=C+D		B			* final answer is now here, so restore A,B,C,D and return

	B=C			W			* get high values (A,B)[B]
	BSLC
	BSLC
	A=B			B			* restore A
	BSLC
	BSLC					* restore B


	CSRC	
	CSRC					* move answer high

	DSR			W
	DSR			A			* move (D,C)[B] low

	C=D			B			* restore C

	DSR			A
	DSR			X			* restored original D

	RTN						* done finally

* end - Misc.s