vfp.c

ARM9基于WINDOWSCE的BSP源代码

                        // Return the software status
                        _controlfp (temp_emctrl, _MCW_RC);
                        break;                                              
                    default:
                        // All other EXTENDED opcodes are RESERVED
                        return FALSE;
                }   
                break;
            default:
                // All other opcodes are RESERVED
                return FALSE;
        }

        // return we should fault already
        if (CheckException (ptrFPStatus->FPSCR, _statusfp ())) {
            break;
        }
        VFPir_UpdateRegisters (ptrFPStatus);
    }   
    
    return TRUE;
}

/*****************************************************************************
*
* int VFPir_DoubleHandler (_FPStruct *ptrFPStatus, PCONTEXT pctx)
*
* Handles the double precision floating point exceptions (or possible 
* exceptions). After excution of each operation the number of exceptions is 
* tested to determine if an exception occurred. If an exception did occur and 
* the exception was enabled by the user the floating point status is saved and 
* passed onto the users handler on exit from the VFP exception handling code.
*
*       ptrFPStatus :   Pointer to the floating point status structure.
*       pctx        :   Pointer to the exception context
*
******************************************************************************/
BOOL VFPir_DoubleHandler (_FPStruct *ptrFPStatus, PCONTEXT pctx)
{
    DoubleIntUnion tmp_dest;
    DoubleIntUnion tmp_rhs;
    SingleIntUnion tmp_rhs_single;

    double dest, rhs, lhs;
    unsigned int i;
    DWORD temp_emctrl;

    for (i = 0; i <= ptrFPStatus->iterations; i++)
    {
        lhs  = VFP_GetRegDouble (ptrFPStatus->Rn, pctx);
        rhs  = VFP_GetRegDouble (ptrFPStatus->Rm, pctx);   
        dest = VFP_GetRegDouble (ptrFPStatus->Rd, pctx);

        switch (ptrFPStatus->opcode)
        {
            case FMAC:      // Fd = Fd + (Fn * Fm)
                rhs = lhs * rhs;
                dest = dest + rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FNMAC:     // Fd = Fd - (Fn * Fm)
                rhs = lhs * rhs;
                dest = dest - rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FMSC:      // Fd = -Fd + (Fn * Fm)
                rhs = lhs * rhs;
                dest = -dest;
                dest = dest + rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FNMSC:     // Fd = -Fd - (Fn * Fm)
                rhs = lhs * rhs;
                dest = -dest;
                dest = dest - rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FMUL:      // Fd = Fn * Fm
                dest = lhs * rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FNMUL:     // Fd = -(Fn * Fm)
                dest = lhs * rhs;
                dest = -dest;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FADD:      // Fd = Fn + Fm
                dest = lhs + rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FSUB:      // Fd = Fn - Fm
                dest = lhs - rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case FDIV:      // Fd = Fn / Fm
                dest = lhs / rhs;
                VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                break;
            case EXTEND:    // Extended operations
                switch (ptrFPStatus->opcode_ext)
                {
                    case FCPY:      // Fd = Fm
                        VFP_SetRegDouble (ptrFPStatus->Rd, rhs, pctx);
                        break;
                    case FABS:      // Fd = abs(Fm)
                        dest = fabs(rhs);
                        VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                        break;
                    case FNEG:      // Fd = -(Fm)
                        dest = -rhs;
                        VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                        break;
                    case FSQRT:     // Fd = sqrt(Fm)
                        dest = sqrt(rhs);
                        VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                        break;
                    case FCMP:      // Flags := Fd <> Fm
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_double = dest;
                        tmp_rhs.as_double = rhs;

                        if (dest == rhs)
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < rhs)
                            ptrFPStatus->FPSCR |= LESS_THAN;                            
                        else if (dest > rhs)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            ptrFPStatus->FPSCR |= UNORDERED;
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception
                            if (!SIGNAL_NaN_D(tmp_dest.as_unsignedint[1], tmp_dest.as_unsignedint[0]) &&
                                !SIGNAL_NaN_D(tmp_rhs.as_unsignedint[1], tmp_rhs.as_unsignedint[0])) {
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }
                        break;
                    case FCMPE:     // Flags := Fd <> Fm with exception reporting
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_double = dest;
                        tmp_rhs.as_double = rhs;

                        if (dest == rhs)
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < rhs)
                            ptrFPStatus->FPSCR |= LESS_THAN;                            
                        else if (dest > rhs)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            ptrFPStatus->FPSCR |= UNORDERED;
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception

                            if (SIGNAL_NaN_D(tmp_dest.as_unsignedint[1], tmp_dest.as_unsignedint[0]) 
                                || QUIET_NaN_D(tmp_dest.as_unsignedint[1], tmp_dest.as_unsignedint[0])
                                || SIGNAL_NaN_D(tmp_rhs.as_unsignedint[1], tmp_rhs.as_unsignedint[0])
                                || QUIET_NaN_D(tmp_rhs.as_unsignedint[1], tmp_rhs.as_unsignedint[0])) {
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }   
                        break;
                    case FCMPZ:     // Flags := Fd <> Zero
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_double = dest;

                        if (dest == (double) 0.0)
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < (double) 0.0)
                            ptrFPStatus->FPSCR |= LESS_THAN;                            
                        else if (dest > (double) 0.0)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            ptrFPStatus->FPSCR |= UNORDERED;
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception
                            if (!SIGNAL_NaN_D(tmp_dest.as_unsignedint[1], tmp_dest.as_unsignedint[0])) {
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }
                        break;
                    case FCMPEZ:    // Flags := Fd <> Zero with exception reporting
                        ptrFPStatus->FPSCR &= 0x0FFFFFFF; // Clear current flags
                        ptrFPStatus->iterations = 0;
                        tmp_dest.as_double = dest;

                        if (dest == (double) 0.0)
                            ptrFPStatus->FPSCR |= EQUAL_TO;
                        else if (dest < (double) 0.0)
                            ptrFPStatus->FPSCR |= LESS_THAN;                            
                        else if (dest > (double) 0.0)
                            ptrFPStatus->FPSCR |= GREATER_THAN;
                        else {
                            ptrFPStatus->FPSCR |= UNORDERED;
                            ptrFPStatus->FPSCR &= ~FPSCR_IE_MASK;   // default no exception

                            if (SIGNAL_NaN_D(tmp_dest.as_unsignedint[1], tmp_dest.as_unsignedint[0])
                                || QUIET_NaN_D(tmp_dest.as_unsignedint[1], tmp_dest.as_unsignedint[0])) {
                                ptrFPStatus->FPSCR |= FPSCR_IOE;
                            }
                        }                               
                        break;
                    case FCVTSD:    // Fd(float reg encoding) = Fm(double reg encoding)
                        ptrFPStatus->iterations = 0;
                        VFP_SetRegSingle (VFPir_DecodeRegister(ptrFPStatus->FPINST, 0, RD_LSB),
                            (float) dest, pctx);
                        break;
                    case FUITOD:    // Fd = ConvertUnsignedIntToDouble Fm (cp 11)
                        ptrFPStatus->iterations = 0;
                        tmp_rhs_single.as_float = VFP_GetRegSingle (
                                    VFPir_DecodeRegister(ptrFPStatus->FPINST, SINGLE_PRECISION, RM_LSB), pctx);

                        dest = (double) tmp_rhs_single.as_unsignedint;
                        VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);
                        break;
                    case FSITOD:    // Fd = ConvertSignedIntToDouble Fm (cp 11)
                        ptrFPStatus->iterations = 0;
                        tmp_rhs_single.as_float = VFP_GetRegSingle (
                                    VFPir_DecodeRegister(ptrFPStatus->FPINST, SINGLE_PRECISION, RM_LSB), pctx);

                        dest = (double) tmp_rhs_single.as_signedint;
                        VFP_SetRegDouble (ptrFPStatus->Rd, dest, pctx);

                        break;
                    case FTOUID:    // Fd = ConvertToUnsignedInt Fm (cp 11 double Current RMODE)
                        ptrFPStatus->iterations = 0;
                        tmp_rhs_single.as_unsignedint = (unsigned int) rhs;
                        VFP_SetRegSingle (VFPir_DecodeRegister(ptrFPStatus->FPINST, SINGLE_PRECISION, RD_LSB),
                                    tmp_rhs_single.as_float, pctx);
                        break;
                    case FTOUIZD:   // Fd = ConvertToUnsignedInt Fm <cp 11 double RZ Mode)
                        ptrFPStatus->iterations = 0;
                        // Set the software status to Round Towards Zero
                        temp_emctrl = _controlfp (_RC_DOWN, _MCW_RC);
                        tmp_rhs_single.as_unsignedint = (unsigned int) rhs;
                        VFP_SetRegSingle (VFPir_DecodeRegister(ptrFPStatus->FPINST, SINGLE_PRECISION, RD_LSB),
                                    tmp_rhs_single.as_float, pctx);
                        // Return the software status
                        _controlfp (temp_emctrl, _MCW_RC);
                        break;                                              
                    case FTOSID:    // Fd = ConvertToSignedInt Fm (cp 11 double Current RMODE)
                        ptrFPStatus->iterations = 0;                        
                        tmp_rhs_single.as_signedint = (int) rhs;
                        VFP_SetRegSingle (VFPir_DecodeRegister(ptrFPStatus->FPINST, SINGLE_PRECISION, RD_LSB),
                                    tmp_rhs_single.as_float, pctx);
                        break;                                          
                    case FTOSIZD:   // Fd = ConvertToSignedInt Fm (cp 11 double RZ mode)
                        ptrFPStatus->iterations = 0;
                        // Set the software status to Round Towards Zero
                        temp_emctrl = _controlfp (_RC_DOWN, _MCW_RC);
                        tmp_rhs_single.as_signedint = (int) rhs;
                        VFP_SetRegSingle (VFPir_DecodeRegister(ptrFPStatus->FPINST, SINGLE_PRECISION, RD_LSB),
                                    tmp_rhs_single.as_float, pctx);
                        // Return the software status
                        _controlfp (temp_emctrl, _MCW_RC);
                        break;                                              
                    default:
                        // All other EXTENDED opcodes are RESERVED
                        return FALSE;
                }   
                break;
            default:
                // All other EXTENDED opcodes are RESERVED
                return FALSE;
        }       

        // return we should fault already
        if (CheckException (ptrFPStatus->FPSCR, _statusfp ())) {
            break;
        }
        VFPir_UpdateRegisters(ptrFPStatus);
    }   
    

    return TRUE;

}