fpparchlib.c

vxworks source code, used for develop vxworks system.

	{
	case FPE_INSN_FADD:
	    {
	    int m = (insn & 0x00f0) >> 4;
	    int n = (insn & 0x0f00) >> 8;

	    if (fpscr & FPSCR_DOUBLE_PRECISION)	/* FADD DRm,DRn: DRn+DRm->DRn */
		{
		UNION64 src, dst;

		if (fppRegGet (m,     &src.u32[HREG], 0) != OK ||
		    fppRegGet (m + 1, &src.u32[LREG], 0) != OK ||
		    fppRegGet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegGet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fadd dr%d,dr%d", m, n, 0, 0, 0);
		printExc (" (0x%08x%08x, 0x%08x%08x)\n",
			  src.u32[HREG], src.u32[LREG], dst.u32[HREG], dst.u32[LREG], 0);
#endif
		dst.u64 = uss_dpadd (dst.u64, src.u64);		/* dst + src */

		if (fppRegSet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegSet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
		}
	    else				/* FADD FRm,FRn: FRn+FRm->FRn */
		{
		UINT32 src, dst;

		if (fppRegGet (m, &src, 0) != OK ||
		    fppRegGet (n, &dst, 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fadd fr%d,fr%d (0x%08x, 0x%08x)\n",m,n,src,dst,0);
#endif
		dst = uss_fpadd (dst, src);			/* dst + src */

		if (fppRegSet (n, &dst, 0) != OK)
		    goto FpError;
		}
	    break;
	    }

	case FPE_INSN_FSUB:
	    {
	    int m = (insn & 0x00f0) >> 4;
	    int n = (insn & 0x0f00) >> 8;

	    if (fpscr & FPSCR_DOUBLE_PRECISION) /* FSUB DRm,DRn: DRn-DRm->DRn */
		{
		UNION64 src, dst;

		if (fppRegGet (m,     &src.u32[HREG], 0) != OK ||
		    fppRegGet (m + 1, &src.u32[LREG], 0) != OK ||
		    fppRegGet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegGet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fsub dr%d,dr%d", m, n, 0, 0, 0);
		printExc (" (0x%08x%08x, 0x%08x%08x)\n",
			  src.u32[HREG], src.u32[LREG], dst.u32[HREG], dst.u32[LREG], 0);
#endif
		dst.u64 = uss_dpsub (dst.u64, src.u64);		/* dst - src */

		if (fppRegSet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegSet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
		}
	    else				/* FSUB FRm,FRn: FRn-FRm->FRn */
		{
		UINT32 src, dst;

		if (fppRegGet (m, &src, 0) != OK ||
		    fppRegGet (n, &dst, 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fsub fr%d,fr%d (0x%08x, 0x%08x)\n",m,n,src,dst,0);
#endif
		dst = uss_fpsub (dst, src);			/* dst - src */

		if (fppRegSet (n, &dst, 0) != OK)
		    goto FpError;
		}
	    break;
	    }

	case FPE_INSN_FMUL:
	    {
	    int m = (insn & 0x00f0) >> 4;
	    int n = (insn & 0x0f00) >> 8;

	    if (fpscr & FPSCR_DOUBLE_PRECISION) /* FMUL DRm,DRn: DRn*DRm->DRn */
		{
		UNION64 src, dst;

		if (fppRegGet (m,     &src.u32[HREG], 0) != OK ||
		    fppRegGet (m + 1, &src.u32[LREG], 0) != OK ||
		    fppRegGet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegGet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fmul dr%d,dr%d", m, n, 0, 0, 0);
		printExc (" (0x%08x%08x, 0x%08x%08x)\n",
			  src.u32[HREG], src.u32[LREG], dst.u32[HREG], dst.u32[LREG], 0);
#endif
		dst.u64 = uss_dpmul (dst.u64, src.u64);		/* dst * src */

		if (fppRegSet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegSet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
		}
	    else				/* FMUL FRm,FRn: FRn*FRm->FRn */
		{
		UINT32 src, dst;

		if (fppRegGet (m, &src, 0) != OK ||
		    fppRegGet (n, &dst, 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fmul fr%d,fr%d (0x%08x, 0x%08x)\n",m,n,src,dst,0);
#endif
		dst = uss_fpmul (dst, src);			/* dst * src */

		if (fppRegSet (n, &dst, 0) != OK)
		    goto FpError;
		}
	    break;
	    }

	case FPE_INSN_FDIV:
	    {
	    int m = (insn & 0x00f0) >> 4;
	    int n = (insn & 0x0f00) >> 8;

	    if (fpscr & FPSCR_DOUBLE_PRECISION)	/* FDIV DRm,DRn: DRn/DRm->DRn */
		{
		UNION64 src, dst;

		if (fppRegGet (m,     &src.u32[HREG], 0) != OK ||
		    fppRegGet (m + 1, &src.u32[LREG], 0) != OK ||
		    fppRegGet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegGet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fdiv dr%d,dr%d", m, n, 0, 0, 0);
		printExc (" (0x%08x%08x, 0x%08x%08x)\n",
			  src.u32[HREG], src.u32[LREG], dst.u32[HREG], dst.u32[LREG], 0);
#endif
		dst.u64 = uss_dpdiv (dst.u64, src.u64);		/* dst / src */

		if (fppRegSet (n,     &dst.u32[HREG], 0) != OK ||
		    fppRegSet (n + 1, &dst.u32[LREG], 0) != OK)
		    goto FpError;
		}
	    else				/* FDIV FRm,FRn: FRn/FRm->FRn */
		{
		UINT32 src, dst;

		if (fppRegGet (m, &src, 0) != OK ||
		    fppRegGet (n, &dst, 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fdiv fr%d,fr%d (0x%08x, 0x%08x)\n",m,n,src,dst,0);
#endif
		dst = uss_fpdiv (dst, src);			/* dst / src */

		if (fppRegSet (n, &dst, 0) != OK)
		    goto FpError;
		}
	    break;
	    }

	case FPE_INSN_FMAC:
	    {
	    int m = (insn & 0x00f0) >> 4;
	    int n = (insn & 0x0f00) >> 8;

	    if (fpscr & FPSCR_DOUBLE_PRECISION)
		{
		goto FpError;		/* FMAC -- NO DOUBLE PRECISION */
		}
	    else			/* FMAC FR0,FRm,FRn: FR0*FRm+FRn->FRn */
		{
		UINT32 fr0, src, dst;

		if (fppRegGet (0, &fr0, 0) != OK ||
		    fppRegGet (m, &src, 0) != OK ||
		    fppRegGet (n, &dst, 0) != OK)
		    goto FpError;
#ifdef FPP_DEBUG
		printExc ("fmac fr0,fr%d,fr%d (0x%08x, 0x%08x, 0x%08x)\n",
			  m,n,fr0,src,dst);
#endif
		src = uss_fpmul (src, fr0);
		dst = uss_fpadd (dst, src);		/* dst + (src * fr0) */

		if (fppRegSet (n, &dst, 0) != OK)
		    goto FpError;
		}
	    break;
	    }

	default:
	    {
	    switch (insn & FPE_MASK_1REG)
		{
		case FPE_INSN_FSQRT:
		    {
		    int n = (insn & 0x0f00) >> 8;

		    if (fpscr & FPSCR_DOUBLE_PRECISION)		/* FSQRT DRn */
			{
			UNION64 dst;

			if (fppRegGet (n,     &dst.u32[HREG], 0) != OK ||
			    fppRegGet (n + 1, &dst.u32[LREG], 0) != OK)
			    goto FpError;
#ifdef FPP_DEBUG
			printExc ("fsqrt dr%d (0x%08x%08x)\n",
				  n, dst.u32[HREG], dst.u32[LREG], 0, 0);
#endif
			dst.u64 = uss_dpsqrt (dst.u64);

			if (fppRegSet (n,     &dst.u32[HREG], 0) != OK ||
			    fppRegSet (n + 1, &dst.u32[LREG], 0) != OK)
			    goto FpError;
			}
		    else					/* FSQRT FRn */
			{
			UINT32 dst;

			if (fppRegGet (n, &dst, 0) != OK)
			    goto FpError;
#ifdef FPP_DEBUG
			printExc ("fsqrt fr%d (0x%08x)\n",n,dst,0,0,0);
#endif
			dst = uss_fpsqrt (dst);

			if (fppRegSet (n, &dst, 0) != OK)
			    goto FpError;
			}
		    break;
		    }

		case FPE_INSN_FCNVSD:	/* FCNVSD FPUL,DRn: Double(FPUL)->DRn */
		    {
		    int n = (insn & 0x0f00) >> 8;

		    if (fpscr & FPSCR_DOUBLE_PRECISION)
			{
			UINT32 fpul;
			UNION64 dst;

			fpul = fppFpulGet ();
#ifdef FPP_DEBUG
			printExc ("fcnvsd fpul,dr%d (0x%08x)\n",n,fpul,0,0,0);
#endif
			dst.u64 = uss_fptodp (fpul);

			if (fppRegSet (n,     &dst.u32[HREG], 0) != OK ||
			    fppRegSet (n + 1, &dst.u32[LREG], 0) != OK)
			    goto FpError;
			}
		    else
			goto FpError;	/* FCNVSD -- NO SINGLE PRECISION */

		    break;
		    }

		case FPE_INSN_FCNVDS:	/* FCNVDS DRm,FPUL: (float)DRm->FPUL */
		    {
		    int m = (insn & 0x0f00) >> 8;

		    if (fpscr & FPSCR_DOUBLE_PRECISION)
			{
			UNION64 src;
			UINT32 fpul;

			if (fppRegGet (m,     &src.u32[HREG], 0) != OK ||
			    fppRegGet (m + 1, &src.u32[LREG], 0) != OK)
			    goto FpError;
#ifdef FPP_DEBUG
			printExc ("fcnvds dr%d,fpul (0x%08x%08x)\n",
				  m, src.u32[HREG], src.u32[LREG], 0, 0);
#endif
			fpul = uss_dptofp (src.u64);

			fppFpulSet (fpul);
			}
		    else
			goto FpError;	/* FCNVDS -- NO SINGLE PRECISION */

		    break;
		    }

		default:
		    goto FpError;	/* UNKNOWN FPU ERROR */
		}
	    }
	}
    return OK;
FpError:
    pEsf->pc = pcSave;		/* restore PC */
    pRegs->pr = prSave;		/* restore PR */
#ifdef FPP_DEBUG
    printExc ("fppExcHandle: PC=0x%x  FPSCR=0x%x \n",(int)pcSave, fpscr, 0,0,0);
#endif
    return ERROR;
    }

/******************************************************************************
*
* fppExcFixup - fix exception stack frame to prepare for resuming FP task
*
* This routine fixes pc (and pr) on exception stack frame, so as the task
* which got exception is able to resume its computation.  For most cases,
* it is just a matter of incrementing pc to skip the emulated FPU instruction.
* However the things are not so simple if the FPU instruction is located in
* a delay slot of branch instruction, since pc points at the branch instruction
* (NOT the FPU instruction) and the next pc depends on the branch operation.
* Namely we have to emulate the branch instruction also.  All the delayed
* branch instructions are listed below:
*
*	----------------------
* 		opcode	 mask
*	----------------------
*	rts	0x000b, 0xffff
*	rte	0x002b, 0xffff
*	bsrf	0x0003, 0xf0ff
*	braf	0x0023, 0xf0ff
*	jsr	0x400b, 0xf0ff
*	jmp	0x402b, 0xf0ff
*	bt/s	0x8d00, 0xff00
*	bf/s	0x8f00, 0xff00
*	bra	0xa000, 0xf000
*	bsr	0xb000, 0xf000
*	----------------------
*
* RETURNS: pointer to FPU instruction which got exception, or
*          NULL if failed to emulate a delayed branch instruction
*
* NOMANUAL
*/

LOCAL INSTR *fppExcFixup
    (
    ESFSH   *pEsf,	/* pointer to exception stack frame */
    REG_SET *pRegs	/* pointer to register info on stack */
    )
    {
    INSTR insn = *pEsf->pc;
    INSTR *pFpeInsn = pEsf->pc + 1;	/* if it is a delayed branch insn */
    INT32 pc = (INT32)pEsf->pc;
    INT32 npc;				/* next pc */
    BOOL updatePr = FALSE;

    if (insn == 0x000b)			/* RTS (PR->PC) */
	{
	npc = (INT32)pRegs->pr;
	}
    else if (insn == 0x002b)		/* RTE (SSR->SR, SPC->PC) */
	{
	/* The original ssr/spc are overwritten by this FPU error exception,
	 * hence we cannot emulate `rte' instruction here...
	 */
	return NULL;
	}
    else if ((insn & 0xf0ff) == 0x0003)	/* BSRF Rn (PC+4->PR, PC+4+Rn->PC) */
	{
	int n = (insn & 0x0f00) >> 8;
	npc = pc + 4 + (n < 8 ? (INT32)pRegs->voreg[n]
			      : (INT32)pRegs->nvreg[n - 8]);
	updatePr = TRUE;
	}
    else if ((insn & 0xf0ff) == 0x0023)	/* BRAF Rn (PC+4+Rn->PC) */
	{
	int n = (insn & 0x0f00) >> 8;
	npc = pc + 4 + (n < 8 ? (INT32)pRegs->voreg[n]
			      : (INT32)pRegs->nvreg[n - 8]);
	}
    else if ((insn & 0xf0ff) == 0x400b)	/* JSR @Rn (PC+4->PR, Rn->PC) */
	{
	int n = (insn & 0x0f00) >> 8;
	npc = n < 8 ? (INT32)pRegs->voreg[n] : (INT32)pRegs->nvreg[n - 8];
	updatePr = TRUE;
	}
    else if ((insn & 0xf0ff) == 0x402b)	/* JMP @Rn (Rn->PC) */
	{
	int n = (insn & 0x0f00) >> 8;
	npc = n < 8 ? (INT32)pRegs->voreg[n] : (INT32)pRegs->nvreg[n - 8];
	}
    else if ((insn & 0xff00) == 0x8d00)	/* BT/S label (PC+4+disp*2->PC) */
	{
	if (pEsf->sr & SR_BIT_T) npc = pc + 4 + (INT8)(insn & 0x00ff) * 2;
	else                     npc = pc + 4;
	}
    else if ((insn & 0xff00) == 0x8f00)	/* BF/S label (PC+4+disp*2->PC) */
	{
	if (pEsf->sr & SR_BIT_T) npc = pc + 4;
	else                     npc = pc + 4 + (INT8)(insn & 0x00ff) * 2;
	}
    else if ((insn & 0xf000) == 0xa000) /* BRA label (PC+4+disp*2->PC) */
	{
	INT16 disp = (insn & 0x0800) ? (insn | 0xf000) : (insn & 0x0fff);
	npc = pc + 4 + disp * 2;
	}
    else if ((insn & 0xf000) == 0xb000) /* BSR label(PC+4->PR,PC+4+disp*2->PC)*/
	{
	INT16 disp = (insn & 0x0800) ? (insn | 0xf000) : (insn & 0x0fff);
	npc = pc + 4 + disp * 2;
	updatePr = TRUE;
	}
    else
	{
	pFpeInsn = pEsf->pc;	/* Good!  FPE insn is not in delay slot, */
	npc = pc + 2;		/* simply skip FPU instruction */
	}

#ifdef FPP_DEBUG
    if (insn == 0x000b)
	printExc ("rts (to 0x%08x)\n",npc,0,0,0,0);
    else if (insn == 0x002b)
	printExc ("rte (cannot emulate)\n",0,0,0,0,0);
    else if ((insn & 0xf0ff) == 0x0003)
	printExc("bsrf r%d (==> 0x%08x)\n",(insn & 0x0f00)>>8,npc,0,0,0);
    else if ((insn & 0xf0ff) == 0x0023)
	printExc ("braf r%d (==> 0x%08x)\n",(insn & 0x0f00)>>8,npc,0,0,0);
    else if ((insn & 0xf0ff) == 0x400b)
	printExc ("jsr @r%d (==> 0x%08x)\n",(insn & 0x0f00)>>8,npc,0,0,0);
    else if ((insn & 0xf0ff) == 0x402b)
	printExc ("jmp @r%d (==> 0x%08x)\n",(insn & 0x0f00)>>8,npc,0,0,0);
    else if ((insn & 0xff00) == 0x8d00)
	printExc ("bt/s 0x%08x\n",npc,0,0,0,0);
    else if ((insn & 0xff00) == 0x8f00)
	printExc ("bf/s 0x%08x\n",npc,0,0,0,0);
    else if ((insn & 0xf000) == 0xa000)
	printExc ("bra 0x%08x\n",npc,0,0,0,0);
    else if ((insn & 0xf000) == 0xb000)
	printExc ("bsr 0x%08x\n",npc,0,0,0,0);
#endif /*FPP_DEBUG*/

    if (updatePr)
	pRegs->pr = (INSTR *)(pc + 4);		/* update PR on stack */

    pEsf->pc = (INSTR *)npc;			/* update PC on stack */

    return pFpeInsn;	/* return pointer to actual FPU instruction */
    }

#endif /* CPU==SH7750 */