
#include <linux/linkage.h>
LOFF_D0 	= 0x00
LOFF_D1 	= 0x04
LOFF_D2 	= 0x08
LOFF_D3 	= 0x0C
LOFF_D4 	= 0x10
LOFF_D5 	= 0x14
LOFF_D6 	= 0x18
LOFF_D7 	= 0x1C
LOFF_A0 	= 0x20
LOFF_A1 	= 0x24
LOFF_A2 	= 0x28
LOFF_A3 	= 0x2C
LOFF_A4 	= 0x30
LOFF_A5 	= 0x34
LOFF_A6		= 0x38   
LOFF_USP	= 0x3C
LOFF_PAD	= 0x40
LOFF_STKADJ	= 0x42
LOFF_SR 	= 0x44
LOFF_PC 	= 0x46
LOFF_FORMAT	= 0x4A
LOFF_EADD	= 0x4C		| only for frame 4 trap
LOFF_STACK	= LOFF_EADD	| for frame 1 (030) it points to SSP
LOFF_PC2	= 0x50		


   	.text
	.globl SYMBOL_NAME(fpu_emu_trap),SYMBOL_NAME(copyin),SYMBOL_NAME(copyout)
	.globl SYMBOL_NAME(fastmultiply)

ENTRY(fpu_emu_trap)
	clrl	%sp@-	
	clrl	%sp@-
	moveml	%d0-%a6,%sp@-
	movel	%usp,%a0
	btst 	#5,%sp@(LOFF_SR)	| check for supervisor mode
	beqs	inuser
	leal	%sp@(LOFF_STACK),%a0	
inuser:
	movew 	%sp@(LOFF_FORMAT),%d0
	andw	#0xf000,%d0
	cmpw	#0x4000,%d0		| are we on LC040 ?
	bnes	noLC040
	movel	%sp@(LOFF_PC2),%sp@(LOFF_PC)	| we can overwrite this value
						| because after emulation it
						| will have been advanced
	btst	#5,%sp@(LOFF_SR)
	beqs 	noLC040
	addql	#8,%a0			| SSP is 8 bytes higher now
noLC040:
	movel	%a0,%sp@(LOFF_USP)				
	movel	%sp,%sp@-
	jbsr	SYMBOL_NAME(do_fpu_emulate)
	addql	#4,%sp
	movel	%sp@(LOFF_USP),%a0
	
	movel	%a0,%usp
exitint:
	moveml	%sp@+,%d0-%a6 	
	addql	#8,%sp		 		
	rte



ENTRY(fastmultiply)
| multiply 2 112 bits mantissas to yield a 224 bits product

             movem.l 	%d1-%d7/%a0-%a6,%sp@-
	movel	60(%sp),%a0	| X
	movel	64(%sp),%a1	| Y
	movel	68(%sp),%a2	| scratch+product



	movew	#7,%d7
clearsum:
	clrl	(%a2)+
	dbra	%d7,clearsum
	
	
	movel	%a2,%a5
	leal	16(%a1),%a4	| y

	
	movew	#3,%d7
outerloop:	movew	%d7,%sp@-
	movel	%a4@-,%d0 	| get Yn
             clrl	%d7	| zero
	clrl	%d5	| clear carry
	clrl	%d4	| clear HiOld
	leal	16(%a0),%a3  | x
	subql	#4,%a5
	movel	%a5,%a2	| scratch pointer

	movew	#3,%d6
innerloop:
	movel	%a3@-,%d2	| get Xn
	clrl	%d1
	mulul	%d0,%d1,%d2
	
	movel	(%a2),%d3

	addl	%d5,%d3
	clrl	%d5
	addxl	%d7,%d5
	
	addl	%d2,%d3
	addxl	%d7,%d5
	
	addl	%d4,%d5
	addxl	%d7,%d5
	
	
	movel	%d1,%d4
	movel	%d3,(%a2)
	subql	#4,%a2
	
	dbra	%d6,innerloop
	
	
	movel	(%a2),%d3

	addl	%d5,%d3
	addl	%d4,%d3
	
	movel	%d3,(%a2)
	movew	%sp@+,%d7	
	dbra	%d7,outerloop
		
	
	movel	68(%sp),%a0
	clrl	%d2
	leal	32(%a0),%a0
	
	movew	#7,%d3
shiftleft:
	movel	%a0@-,%d0	
	roll	#8,%d0
	movel	%d0,%d1
	andl	#0xffffff00,%d0
	orl	%d2,%d0
	movel	%d1,%d2
	andl	#0x000000ff,%d2	
	
	movel	%d0,(%a0)
	dbra	%d3,shiftleft
	
	movel	68(%sp),%a0
	lea 	32(%a0),%a0
	clrl	%d2
	movew	#7,%d3
shiftleft2:
	movel	%a0@-,%d0	
	roll	#6,%d0
	movel	%d0,%d1
	andl	#0xffffffc0,%d0
	orl	%d2,%d0
	movel	%d1,%d2
	andl	#0x0000003f,%d2	
	
	movel	%d0,(%a0)
	dbra	%d3,shiftleft2
			
	movel	68(%sp),%a0
	leal	16(%a0),%a0
	clrl	%d0
	orl	%a0@+,%d0
	orl	%a0@+,%d0
	orl	%a0@+,%d0
	orl	%a0@+,%d0
	orl	%a0@+,%d0
	orl	%a0@+,%d0
	orl	%a0@+,%d0
	orl	%a0@+,%d0	| calc sticky
	tstl	%d0
	sne	%d0
	andil	#1,%d0

	
	movem.l 	%sp@+,%d1-%d7/%a0-%a6
	rts
	


ENTRY(fusword)
             movem.l	%a0-%a1,%sp@-
	movel 	12(%sp),%a0
	movel	SYMBOL_NAME(globlframe),%a1
	movew	%a1@(LOFF_SR),%d0
	andl	#0x2000,%d0
	bnes	superread

|	movel	#1,%d0
|	movec	%d0,%SFC


	movesw	(%a0),%d0
	bras	fusexit	
superread:	
          	movew	(%a0),%d0
fusexit:
          	movem.l	%sp@+,%a0-%a1
          	rts
          	

ENTRY(copyout)
             movem.l 	%d0-%d1/%a0-%a1,%sp@-
	movel	20(%sp),%a0	| source
	movel	24(%sp),%a1	| destination
	movel	28(%sp),%d0	| count
	subql	#1,%d0		| dec count by 1 for dbra

|	movel	#1,%d1
|	movec	%d1,%DFC		| set dfc for user data space

moreout:
	moveb	(%a0)+,%d1	| fetch supervisor byte
	movesb	%d1,(%a1)+	| write user byte
	dbf	%d0,moreout
	movem.l 	%sp@+,%d0-%d1/%a0-%a1
	rts

ENTRY(copyin)
	movem.l 	%d0-%d1/%a0-%a1,%sp@-
	movel	20(%sp),%a0	| source
	movel	24(%sp),%a1	| destination
	movel	28(%sp),%d0	| count
	subql	#1,%d0		| dec count by 1 for dbra

|	movel	#1,%d1
|	movec	%d1,%SFC		| set sfc for user space

morein:
	movesb	(%a0)+,%d1	| fetch user byte
	moveb	%d1,(%a1)+	| write supervisor byte
	dbf	%d0,morein
	movem.l 	%sp@+,%d0-%d1/%a0-%a1
	rts


