shfloat.c

windows ce 3.00 嵌入式操作系统源代码

/* Copyright (c) 1999-2000 Microsoft Corporation.  All rights reserved. */
#ifdef SH4
#include "kernel.h"
#include "shxinst.h"
// fp instruction opcodes
#define FADD_OP   0x0
#define FSUB_OP   0x1
#define FMUL_OP   0x2
#define FDIV_OP   0x3
#define FCMP_EQ   0x4
#define FCMP_GT   0x5
#define FMAC_OP   0xe

// all these instrs have a last 
// common opcode is 0xd
#define FLOAT_OP  0x2
#define FTRC_OP   0x3
#define FSQRT_OP  0x6
#define FCNVSD_OP 0xa
#define FCNVDS_OP 0xb
#define FIPR_OP   0xe
#define FTRV_OP   0xf

// fpscr bit masks
#define FR              0x00200000       // FR
#define SZ              0x00100000       // SZ
#define PR              0x00080000       // PR
#define DN              0x00040000       // DN

#define IMCW_ENABLE     0x00000f80       // Enable mask
#define E_INVALID       0x00000800       // Invalid operation
#define E_ZERODIVIDE    0x00000400       // Divide by zero
#define E_OVERFLOW      0x00000200       // Overflow
#define E_UNDERFLOW     0x00000100       // Underflow
#define E_INEXACT       0x00000080       // Inexact result

#define IMCW_CAUSE	0x0001f000	 // Cause mask
#define C_INVALID       0x00010000	 // Invalid operation
#define C_ZERODIVIDE    0x00008000	 // Divide by 0
#define C_OVERFLOW      0x00004000	 // Overflow
#define C_UNDERFLOW     0x00002000	 // Underflow
#define C_INEXACT       0x00001000	 // Inexact result

#define IMCW_FLAG	0x0000007c	 // Flag mask

extern float _adds(float,float);
extern float _subs(float,float);
extern float _muls(float,float);
extern float _divs(float,float);
extern int _eqs(float,float);
extern int _gts(float,float);

extern double _addd(double, double);
extern double _subd(double, double);
extern double _muld(double, double);
extern double _divd(double, double);
extern int _eqd(double,double);
extern int _gtd(double,double);

extern float _itos(int);
extern double _itod(int);
extern int _stoi(float);
extern int _dtoi(double);
extern double sqrt(double);
extern float  fsqrt(float);
extern double _stod(float);
extern float  _dtos(double);

extern ULONG __asm1(const char *,...);

typedef struct _FP_DOUBLE_OPERAND {
    union {
        struct {
            ULONG low;
            LONG  high;
        };
        double d;
    };
} FP_DOUBLE_OPERAND, *PFP_DOUBLE_OPERAND;

typedef union _FP_SINGLE_OPERAND {
    ULONG i;
    float f;
} FP_SINGLE_OPERAND, *PFP_SINGLE_OPERAND;


void set_fpscr(ULONG mask)
{
    __asm("sts  fpscr, r0\n"
          "or   r4, r0\n"
          "lds  r0, fpscr\n",
          mask);
}

void clr_fpscr(ULONG mask)
{
    __asm("sts fpscr, r0\n"
          "not r4, r4\n"
          "and r4, r0\n"
          "lds r0, fpscr\n",
          mask);
}

ULONG get_cause()
{
    return __asm1("sts fpscr, r0\n"
                  "and r4,r0\n"
                  "shlr8 r0\n"
                  "shlr2 r0\n"
                  "shlr2 r0\n",
                  0x3f00);
}

ULONG get_fpscr(void)
{
    return __asm1("sts fpscr,r0\n");
}

void fipr(ULONG *pFVm, ULONG *pFVn)
{
    float result=0.0f;
    FP_SINGLE_OPERAND fp1,fp2;

    fp1.i = (*pFVm)++;
    fp2.i = (*pFVn)++;
    result = _muls(fp1.f,fp2.f);
    
    fp1.i = (*pFVm)++;
    fp2.i = (*pFVn)++;
    fp2.f = _muls(fp1.f,fp2.f);
    result = _adds(fp2.f,result);

    fp1.i = (*pFVm)++;
    fp2.i = (*pFVn)++;
    fp2.f = _muls(fp1.f,fp2.f);
    result = _adds(fp2.f,result);

    fp1.i = (*pFVm);
    fp2.i = (*pFVn);
    fp2.f = _muls(fp1.f,fp2.f);
    fp2.f = _adds(fp2.f,result);
    *pFVn = fp2.i;
}

void ftrv(ULONG *pXMTRX, ULONG *pFVn)
{
    int i;
    float result=0.0f;
    FP_SINGLE_OPERAND fp1,fp2;
    ULONG *pResult = pFVn;
    ULONG *pFV = pFVn;

    for (i=0; i < 4; i++)
    {
        result = 0.0f;
        fp1.i = (*pXMTRX)++;
        fp2.i = (*pFV)++;
        fp2.f =  _muls(fp1.f,fp2.f);
        result = _adds(fp2.f,result);

        fp1.i = (*pXMTRX)++;
        fp2.i = (*pFV)++;
        fp2.f = _muls(fp1.f,fp2.f);
        result = _adds(fp2.f,result);

        fp1.i = (*pXMTRX)++;
        fp2.i = (*pFV)++;
        fp2.f = _muls(fp1.f,fp2.f);
        result = _adds(fp2.f,result);

        fp1.i = (*pXMTRX)++;
        fp2.i = (*pFV);
        fp2.f = _muls(fp1.f,fp2.f);
        fp2.f = _adds(fp2.f,result);
        //
        // store result to FV[n,n+1,n+2,n+3]
        //
        *pResult++ = fp2.i;
        //
        // reset the pFV to beginning of pFVn
        //
        pFV = pFVn;
    }
}

BOOL HandleHWFloatException(EXCEPTION_RECORD *ExceptionRecord,
                            PCONTEXT pctx) 
{
    PVOID ExceptionAddress;
    SH3IW Instruction;
    ULONG OpcodeExt, RegM, RegN, Opcode;
    ULONG prbit, frbit, CauseField;

    FP_DOUBLE_OPERAND dp1,dp2;
    FP_SINGLE_OPERAND fp1,fp2;
    ULONG *fpRegN1, *fpRegM1, *fpRegN2, *fpRegM2;
    ULONG *pFVn, *pFVm, *pXMTRX;

    char f[10];
    void *function = f;

    int retVal = 1;

    ExceptionAddress = ExceptionRecord->ExceptionAddress;

    __try {
        __try {

            Instruction = *(PSH3IW)ExceptionRecord->ExceptionAddress;
            OpcodeExt = Instruction.instr3.ext_disp;
            RegM = Instruction.instr3.regM;
            RegN = Instruction.instr3.regN;
            Opcode = Instruction.instr3.opcode;

            // check for double operation
            prbit =  ((pctx->Fpscr & PR) != 0);

            // check for float-point bank operation
            frbit =  ((pctx->Fpscr & FR) != 0);

            // reset fpscr to thread state
            clr_fpscr(0xffffffff);
            set_fpscr(pctx->Fpscr);

            // clear PR bit
            clr_fpscr(PR);

            if(!frbit)
            {
                fpRegN1 = pctx->FRegs + RegN;
                fpRegM1 = pctx->FRegs + RegM;

                if (prbit)
                {
                    fpRegN2 = pctx->FRegs + RegN+1;
                    fpRegM2 = pctx->FRegs + RegM+1;
                }                
            }else {
                fpRegN1 = pctx->xFRegs + RegN;
                fpRegM1 = pctx->xFRegs + RegM;

                if (prbit)
                {
                    fpRegN2 = pctx->xFRegs + RegN + 1;
                    fpRegM2 = pctx->xFRegs + RegM + 1;
                }
            }
            
            switch(OpcodeExt)
            {
            case FADD_OP:
            case FSUB_OP:
            case FMUL_OP:
            case FDIV_OP:

                if(prbit)
                {
                    dp1.high = *fpRegN1;
                    dp1.low  = *fpRegN2;
                    dp2.high = *fpRegM1;
                    dp2.low  = *fpRegM2;

                    switch(OpcodeExt)
                    {
                    case FADD_OP:
                        function = (void*)&_addd;
                        break;
                    case FSUB_OP:    
                        function = (void*)&_subd;
                        break;
                    case FMUL_OP:    
                        function = (void*)&_muld;
                        break;
                    case FDIV_OP:
                        function = (void*)&_divd;
                        break;
                    case FCMP_EQ:
                        function = (void*)&_eqd;
                        break;
                    case FCMP_GT:
                        function = (void*)&_gtd;
                        break;
                    }

                    if ((OpcodeExt == FCMP_EQ) || (OpcodeExt == FCMP_GT))
                    {
                        dp2.low = ((int(*)(double,double))(function))(dp1.d,dp2.d);

                        // set user t-bit to comparison result
                        pctx->Psr &= (0xfffffffe & (dp2.low != 0));
                        
                    }else {
                    
                        dp2.d = ((double(*)(double,double))(function))(dp1.d,dp2.d);