fpsoft.s

VxWorks BSP框架源代码包含头文件和驱动

/* fpSoft.s - MIPS R-Series floating point software emulation library */

/* Copyright 1984-2001 Wind River Systems, Inc. */
	.data
	.globl	copyright_wind_river

/*
 * This file has been developed or significantly modified by the
 * MIPS Center of Excellence Dedicated Engineering Staff.
 * This notice is as per the MIPS Center of Excellence Master Partner
 * Agreement, do not remove this notice without checking first with
 * WR/Platforms MIPS Center of Excellence engineering management.
 */

/*
modification history
--------------------
01q,01aug01,mem  Diab integration
01p,16jul01,ros  add CofE comment
01o,20jun01,ros  fix floating point
01n,02may01,mem  Fix rt_cp_2w calculation.
01m,13mar01,tlc  Perform HAZARD review.
01l,05sep00,dra  Updated FP emulation, added 32 FP support.
01k,13jun00,dra  correct cdf file discrepancies
01j,28feb00,dra  Added macros for assembler label definitions.
01i,05oct99,dra  Added 32 FP reg mode support.
01h,16nov95,mem  Changed (SIGNBIT>>16) to (SIGNBIT>>16) & 0xffff to suppress
		 an assembler warning.
		 Fixed FP instruction decoding.  Fixed several bugs in
		 xxx_fmt_tab offset calculations.
01g,18oct93,cd   added R4000 support.
01f,16aug93,yao  added mathSoftAInit(). added store to fpu register from
		 processor, store to processor register to fpu register,
		 load/store to memory address from fpu registger emulation.
		 redefined stack offset constants. changed entry table in
		 data segment to jump table in text segment.
01e,06jun92,ajm  ifdefd out unused code for now, rid of single quotes for
		 make problems, fixed if/else comments
01d,26may92,rrr  the tree shuffle
		  -changed includes to have absolute path from h/
01c,04oct91,rrr  passed through the ansification filter
		  -changed includes to have absolute path from h/
		  -changed ASMLANGUAGE to _ASMLANGUAGE
		  -changed copyright notice
01b,29aug91,wmd   moved to r3k/math directory, chgd softFp.h->fpSoft.h.
01a,22jul91,ajm   wrs-ized from MIPS code
*/

/*
DESCRIPTION
This library contains the MIPS floating point emulation package.
These routines are used for software emulation of floating point instructions,
unimplemented instruction emulation, and correct processing of
ieee FPA exceptions.  This does not emulate and branch on condition 
instructions.  The software floating-point emulator is called from fpIntr 
with floating-point coprocessor unusable exception or the floating-point 
exception.

The register state on entry to this routine is:

    a0 - exception stack pointer
    a1 - fp instruction to be emulated
    a2 - interrupt or exception
    a3 - pFpContext

The normal calling convention is assumed with the appropriate registers
saved by the caller as if it were a high level language routine.

If register a2 it is non-zero then this routine was called from the
floating-point interrupt handler.  In this case the values of the floating
point registers are still in the coprocessor and the pointer to the tcb
structure for the task that executed the fp instruction is in pFpContext.

If register a2 is zero then this routine was called from the coprocessor
unusable handler.  In this case the values of the floating point registers
are in the tcb for the current task and the pointer to the tcb structure
for the current task is in pFpContext.

This routune returns a non-zero value in v0 if there was a signal posted
to the process as the result of an exception.  Otherwise v0 will be zero.
*/

/* |-----------------------------------------------------------|
 * | Copyright (c) 1991, 1990 MIPS Computer Systems, Inc.      |
 * | All Rights Reserved                                       |
 * |-----------------------------------------------------------|
 * |          Restricted Rights Legend                         |
 * | Use, duplication, or disclosure by the Government is      |
 * | subject to restrictions as set forth in                   |
 * | subparagraph (c)(1)(ii) of the Rights in Technical        |
 * | Data and Computer Software Clause of DFARS 252.227-7013.  |
 * |         MIPS Computer Systems, Inc.                       |
 * |         950 DeGuigne Avenue                               |
 * |         Sunnyvale, California 94088-3650, USA             |
 * |-----------------------------------------------------------|
 */

#define _ASMLANGUAGE
#include "vxWorks.h"
#include "esf.h"
#include "fppLib.h"
#include "iv.h"
#include "asm.h"
#include "arch/mips/fpSoft.h"
#include "signal.h"
#include "private/taskLibP.h"

#define DEBUG	

	.globl	GTEXT(mathSoftAInit)
	.globl	GTEXT(softFp)
	.globl	GTEXT(sqrt_s)
	.globl	GTEXT(sqrt_d)
	.globl	GTEXT(abs_s)
	.globl	GTEXT(abs_d)
	.globl	GTEXT(mov_d)
	.globl	GTEXT(mov_s)
	.globl	GTEXT(neg_d)
	.globl	GTEXT(neg_s)
	.globl	GTEXT(add_d)
	.globl	GTEXT(add_s)
	.globl	GTEXT(mul_d)
	.globl	GTEXT(mul_s)
	.globl	GTEXT(div_d)
	.globl	GTEXT(div_s)
	.globl	GTEXT(norm_s)
	.globl	GTEXT(norm_d)
	.globl	GTEXT(cvts_from_d)
	.globl	GTEXT(cvtd_from_s)
	.globl	GTEXT(cvtd_from_w)
	.globl	GTEXT(cvtd_from_l)	
	.globl	GTEXT(cvtw_from_d)
	.globl	GTEXT(cvtw_from_s)

#ifdef DEBUG
	.comm	_fp_sign,4
	.comm	_fp_exp,4
	.comm	_fp_frA,4
	.comm	_fp_frB,4
	.comm	_fp_insn, 4
	.comm	_fp_trace, 4
	.comm	_fp_trace_data, 64
	.comm	_fp_val, 4
	.comm	_fp_fmt, 4
	.comm	_fp_rs,8
	.comm	_fp_rt,8
	.comm	_fp_overflow_trace,4
	.comm	_fp_overflow, 4
	.comm	_fp_rw,4
	.comm	_fp_rd,8
	.comm	_fp_rs_reg,4
	.comm	_fp_rt_reg,4
	.comm	_fp_rd_reg,4
	.comm	_fp_rd_val,8
#endif

	.text
	.set	reorder

/*******************************************************************************
*
* mathSoftAInit - initialize coprocessor as unusable for software floating
*		  point emulation
*
* NOMANUAL
*/
	.globl	GTEXT(mathSoftAInit)

	.ent	mathSoftAInit

FUNC_LABEL(mathSoftAInit)
	mfc0	t0, C0_SR
	HAZARD_CP_READ
	and	t0, t0, ~SR_CU1
	mtc0	t0, C0_SR
	HAZARD_CP_WRITE
	j	ra
	.end	mathSoftAInit

/*******************************************************************************
*
* softFp - software emulation package
*
*/

	.globl	GTEXT(softFp)
	.ent	softFp, 0
FUNC_LABEL(softFp)
	SETFRAME(softFp,1);
#define RM_OFFSET FRAMER0(softFp)
	.frame	sp, FRAMESZ(softFp), ra
	subu	sp, FRAMESZ(softFp)
	SW	ra, FRAMERA(softFp)(sp)	/* save return address */
	sw	a0, FRAMEA0(softFp)(sp)	/* save exception vector */
	sw	a3, FRAMEA3(softFp)(sp)	/* save pFpContext */

#ifdef DEBUG
	sw	a1, _fp_insn
#endif	
	srl	v0, a1, FPU_TYPE_SHIFT	/* shift fpu instruction */
	and	v1, v0, FPU_TYPE_MASK	/* get instruction type */
	beq	v1, zero, emuLdSt	/* check for load/store instruction */

/*
 * In decoding the instruction it is assumed that the opcode field is COP1
 * and that bit 25 is set  (since only floating-point ops are supposed to
 * be emulated in this routine) and therefore these are not checked.
 * The following fields are fully decoded and reserved encodings will result
 * in an illegal instruction signal (SIGILL) being posted:
 *      FMT -- (bits 24-21)
 *     FUNC -- (bits 5-0)
 */
	/*
	 * Extract the FMT field and exclude illegal values.
	 * On exit from this block of code, v0 will have one of
	 * the following values:
	 *	 0	single floating-point
	 *	 8	double floating-point
	 *	16	reserved
	 *	24	reserved
	 *	32	single fixed-point
	 *	40	double fixed-point
	 * which means each jump table for fmt cases needs 6 entries.
	 */
	srl	v0,a1,C1_FMT_SHIFT-3
	and	v0,C1_FMT_MASK<<3
#ifdef DEBUG
	sw	v0, _fp_fmt
#endif
	bgt	v0,C1_FMT_MAX<<3,illfpinst

	/*
	 * Load the floating point value from the register specified by the
	 * RS field into gp registers.  The gp registers which are used for
	 * the value specified by the RS field are dependent on the FMT (v0)
	 * as follows:
	 * 	single		t2
	 *	double		t2,t3
	 *	extended	t2,t3,s2,s3	(where t3 is really zero)
	 *	quad		t2,t3,s2,s3
	 *
	 * Also load the value of the floating-point control and status
	 * register (C1_SR) into gp register a3.
	 */
load_rs:
	srl	v1,a1,RS_SHIFT		# get the RS field
	and	v1,RS_MASK
#ifdef DEBUG
	sw	v1,_fp_rs_reg
#endif

	/*
	 * If a2 (int or exception) is non-zero then the floating-point values
	 * are loaded from the coprocessor else they are loaded from the tcb.
	 */
	beq	a2,zero,rs_tcb
	
	/*
	 * At this point the floating-point value for the specified FPR register
	 * in the RS field (v1) will be loaded from the coprocessor registers
	 * for the FMT specified (v0).  Also the floating-point control and
	 * status register (C1_SR) is loaded into gp register a3.
	 */
	.set	noreorder
	nop				/* BDSLOT */
	cfc1	a3,C1_SR
	HAZARD_CP_READ
					/* setup to branch to the code to load */
	la	t9, cp_rs_fmt_tab	/* load table address  */
	and	t2,a3,CSR_RM_MASK	/* isolate the current Rounding Mode */
	addu	t9, v0, t9
	j	t9			/*  cp for the specified format. */
	SW	t2,RM_OFFSET(sp)	/* (BDSLOT) save current RM on stack */
	.set	reorder
	.set	noreorder
cp_rs_fmt_tab:
	b	rs_cp_1w; nop		/* 16: single */
	b	rs_cp_2w; nop		/* 17: double */
	b	illfpinst; nop		/* 18: reserved */
	b	illfpinst; nop		/* 19: reserved */
	b	rs_cp_1w; nop		/* 20: single (fixed) */
	b	rs_cp_2w; nop		/* 21: double (fixed) */
	.set	reorder

/*******************************************************************************
*
* emuLdSt - emulate FPU load / store instrunction
*
* on entry: a0 - esf stack pointer
*           a1 - FPU instruction caused exception
*           v0 - shifted FPU instruction
*
* registers used: v1, t2, t3
*
* NOMANUAL
*/

emuLdSt:
	andi 	v1, v0, FPU_I_INST_MASK	/* instruction code to v0 */
	andi 	t1, a1, FPU_I_RS_MASK	/* rs field to t1 */
	andi 	t2, v0, FPU_I_RT_MASK	/* rt field to t2 */
	sll	t1, t1, 2		/* register * 4 */
	sll	t2, t2, 2		/* register * 4 */
	lw	a0, FRAMEA0(softFp)(sp)	/* a0=pEsf */
	lw	a2, FRAMEA3(softFp)(sp)		/* a2=pFpContext */
	li   	t3, FPU_INST_MFC
	beq	t3, v1, emuMCfc1	/* is it mfc1 */
	li   	t3, FPU_INST_MTC
	beq	t3, v1, emuMCtc1	/* is it mtc1 */
	li   	t3, FPU_INST_CFC
	beq	t3, v1, emuMCfc1	/* is it cfc1 */
	li   	t3, FPU_INST_CTC
	beq	t3, v1, emuMCtc1	/* is it ctc1 */
	andi	t1, v0, SLW_BASE_MASK	/* else s/lwc1, base to t1 */
	andi	t0, a1, OFFSET_MASK	/* t0 - offset */
	andi	t2, v0, FPU_I_RT_MASK	/* ft field to t2 */
	li	t4, FPU_SW_INSTR
	andi	t6, v0, FPU_STLD1_MASK	/* t6=func code */
	srl	t1, t1, 3		/* base register times 4 */
	sll	t2, t2, 2		/* ft register times 4 */
	beq	t4, t6, emuSw		/* is swc1 instruction */
	li	t4, FPU_LW_INSTR
	beq	t4, t6, emuLw		/* is lwc1 instruction */
emuExit:
	LW	ra, FRAMERA(softFp)(sp)		/* else unknown instruction */
	addu	sp, FRAMESZ(softFp)
	move	v0, zero		/* clear return value */
	j	ra

/*******************************************************************************
*
* emuMfc1 - emulate mfc instruction
*
* on entry: t1 - rs field, t2 - rt field, a0 - pEsf, 
*	    a1 - FPU instr a2 - taskIdCurrent
*
* NOMANUAL
*/

emuMCfc1:
	lw	t3, taskIdCurrent		/* get tcb */
	addi	t2, WIND_TCB_GREG_BASE		/* get gp register offset */
	addu	t1, a3				/* get fpu register address */
	addu	t2, t3				/* gp register address in tcb */
	lw	t4, (t1)			/* load FPU register */
	SW	t4, (t2)			/* store to general register */
	b	emuExit

/*******************************************************************************
*
* emuMCtc1 - emulate mtc instruction
*
* on entry: t1 - rs field, t2 - rt field
*
* NOMANUAL
*/
emuMCtc1:
	addi	t2, t2, E_STK_ZERO	/* get gp offset to esf stack */
	addu	t1, a3, t1		/* get fpu register address */
	addu	t2, a0, t2		/* get gp register address on stack */
	LW	t4, (t2)		/* load general register */
	sw	t4, (t1) 		/* store to fpu register */
	b	emuExit

/*******************************************************************************
*
* emuSw - emulate swc1 instruction
*
* on entry: t0 - offset, t1 - base field, t2 - ft field, a0 - pEsf, 
*	    a1 - FPU instr a3 - pFpContext
*/
emuSw:
	addi	t1, E_STK_ZERO 		/* base register offset to t1 */
	addu	t2, a3			/* fpu register address to t2 */
	addu	t1, a0			/* base address to t1 */
	lw	t3, (t2)		/* load fpu register to t3 */
	LW	t4, (t1)		/* base register to t4 */
	addu	t4, t0, t4		/* destination address to t1 */
	sw	t3, (t4)		/* store fpu register */
	b emuExit

/*******************************************************************************
*
* emuLw - emulate mfc instruction
*
* on entry: t0 - offset, t1 - base field, t2 - ft field, a0 - pEsf, 
*	    a1 - FPU instr a3 - pFpContext
*/
emuLw:
	lw	t4, taskIdCurrent	/* tcb to t4 */
	addu	t2, a3			/* fpu register address to t2 */
	addi	t4, WIND_TCB_GREG_BASE	/* gp base address to t4 */
	addu	t4, t1			/* get base register address */
	LW	t1, (t4)		/* base register to t1 */
	addu	t1, t0, t1		/* source address to t1 */
	lw	t3, (t1)		/* load from source */
	sw	t3, (t2)		/* store to fpu register */
	b	emuExit

/*******************************************************************************
*
* rs_cp_1w -
*
* Load the one word from the coprocessor for the FPR register specified by
* the RS (v1) field into GPR register t2.
*/
rs_cp_1w:
	sll	v1, v1, 3		/* 8 bytes per entry */
	la	t9, rs_cp_1w_tab	/* load table address */
	addu	v1, t9, v1		/* get entry address */
	j	v1

	.set noreorder
rs_cp_1w_tab:
	b	rs_cp_1w_fpr0; nop
	b	rs_cp_1w_fpr1; nop
	b	rs_cp_1w_fpr2; nop
	b	rs_cp_1w_fpr3; nop
	b	rs_cp_1w_fpr4; nop
	b	rs_cp_1w_fpr5; nop
	b	rs_cp_1w_fpr6; nop
	b	rs_cp_1w_fpr7; nop
	b	rs_cp_1w_fpr8; nop
	b	rs_cp_1w_fpr9; nop
	b	rs_cp_1w_fpr10; nop
	b	rs_cp_1w_fpr11; nop
	b	rs_cp_1w_fpr12; nop
	b	rs_cp_1w_fpr13; nop
	b	rs_cp_1w_fpr14; nop
	b	rs_cp_1w_fpr15; nop
	b	rs_cp_1w_fpr16; nop
	b	rs_cp_1w_fpr17; nop
	b	rs_cp_1w_fpr18; nop
	b	rs_cp_1w_fpr19; nop
	b	rs_cp_1w_fpr20; nop
	b	rs_cp_1w_fpr21; nop
	b	rs_cp_1w_fpr22; nop
	b	rs_cp_1w_fpr23; nop
	b	rs_cp_1w_fpr24; nop
	b	rs_cp_1w_fpr25; nop
	b	rs_cp_1w_fpr26; nop
	b	rs_cp_1w_fpr27; nop
	b	rs_cp_1w_fpr28; nop
	b	rs_cp_1w_fpr29; nop
	b	rs_cp_1w_fpr30; nop
	b	rs_cp_1w_fpr31; nop
	.set	reorder

	.set	noreorder
rs_cp_1w_fpr0:
	mfc1	t2,$f0;		b	load_rs_done; 	nop
rs_cp_1w_fpr1:
	mfc1	t2,$f1;		b	load_rs_done; 	nop
rs_cp_1w_fpr2:
	mfc1	t2,$f2;		b	load_rs_done; 	nop
rs_cp_1w_fpr3:
	mfc1	t2,$f3;		b	load_rs_done; 	nop
rs_cp_1w_fpr4:
	mfc1	t2,$f4;		b	load_rs_done; 	nop
rs_cp_1w_fpr5:
	mfc1	t2,$f5;		b	load_rs_done; 	nop
rs_cp_1w_fpr6:
	mfc1	t2,$f6;		b	load_rs_done; 	nop
rs_cp_1w_fpr7:
	mfc1	t2,$f7;		b	load_rs_done; 	nop
rs_cp_1w_fpr8:
	mfc1	t2,$f8;		b	load_rs_done; 	nop
rs_cp_1w_fpr9:
	mfc1	t2,$f9;		b	load_rs_done; 	nop
rs_cp_1w_fpr10:
	mfc1	t2,$f10;	b	load_rs_done; 	nop
rs_cp_1w_fpr11:
	mfc1	t2,$f11;	b	load_rs_done; 	nop