/*---------------------------------------------------------------------------*
  Project: floating point exception demo
  File:    fpe.c

  Copyright 2002 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Log: fpe.c,v $
  Revision 1.2  02/20/2006 04:13:11  mitu
  changed include path from dolphin/ to revolution/.

  Revision 1.1  01/13/2006 11:24:13  hiratsu
  Initial check in.

    
    6     9/02/02 9:04 Shiki
    Modified not to use variables that are declared in SN <math.h>.

    5     8/29/02 13:54 Shiki
    Added #ifdef __MWERKS__ for __frsqrte().

    4     8/26/02 22:31:00 Shiki
    Added test using __OSFpscrEnableBits.

    3     8/22/02 17:46:00 Shiki
    Fixed Func().

    2     8/20/02 09:32:00 Shiki
    Clean up.

    1     8/19/02 21:57 Shiki
    Initial check-in.
  $NoKeywords: $
 *---------------------------------------------------------------------------*/

#include <math.h>
#include <revolution/os.h>
#include <revolution/base/PPCArch.h>

#define FPSCR_VOUZXE    (FPSCR_XE | FPSCR_ZE | FPSCR_UE | FPSCR_OE | FPSCR_VE)
#define FPSCR_VXALL     (FPSCR_VXCVI | FPSCR_VXSQRT | FPSCR_VXSOFT |    \
                         FPSCR_VXVC | FPSCR_VXIMZ | FPSCR_VXZDZ |       \
                         FPSCR_VXIDI | FPSCR_VXISI | FPSCR_VXSNAN)

#ifdef  __MWERKS__
#pragma global_optimizer off
#endif

static OSThread ThreadA;
static u8       ThreadStackA[8192];
static OSThread ThreadB;
static u8       ThreadStackB[8192];

f32 Zero;     // 0.0f;
f32 LargeNum; // 1.0e30f;
f32 SmallNum; // 1.0e-30f;
f32 Nan;      // Zero / Zero;
f32 F;        // 1.1f;
f32 Inf;      // LargeNum * LargeNum;
f32 Neg;      // -1.0f;
f32 Snan;     // Signaling NaN

static void FloatingPointErrorHandler(OSError error, OSContext* context, u32 dsisr, u32 dar)
{
    #pragma unused(error, dsisr, dar)
    u32 fpscr;

    // Mask out FPSCR_*X bit of no interest
    fpscr = context->fpscr;
    fpscr &= ((fpscr & FPSCR_VOUZXE) << (FPSCR_VE_BIT - FPSCR_VX_BIT)) | FPSCR_VXALL;

    if (fpscr & FPSCR_VX)
    {
        OSReport("FPE: Invalid operation: ");
        if (fpscr & FPSCR_VXSNAN)
        {
            OSReport("SNaN\n");
        }
        if (fpscr & FPSCR_VXISI)
        {
            OSReport("Infinity - Infinity\n");
        }
        if (fpscr & FPSCR_VXIDI)
        {
            OSReport("Infinity / Infinity\n");
        }
        if (fpscr & FPSCR_VXZDZ)
        {
            OSReport("0 / 0\n");
        }
        if (fpscr & FPSCR_VXIMZ)
        {
            OSReport("Infinity * 0\n");
        }
        if (fpscr & FPSCR_VXVC)
        {
            OSReport("Invalid compare\n");
        }
        if (fpscr & FPSCR_VXSOFT)
        {
            OSReport("Software request\n");
        }
        if (fpscr & FPSCR_VXSQRT)
        {
            OSReport("Invalid square root\n");
        }
        if (fpscr & FPSCR_VXCVI)
        {
            OSReport("Invalid integer convert\n");
        }
    }
    if (fpscr & FPSCR_OX)
    {
        OSReport("FPE: Overflow\n");
    }
    if (fpscr & FPSCR_UX)
    {
        OSReport("FPE: Underflow\n");
    }
    if (fpscr & FPSCR_ZX)
    {
        OSReport("FPE: Zero division\n");
    }
    if (fpscr & FPSCR_XX)
    {
        OSReport("FPE: Inexact result\n");
    }

    // Move on to the next instruction anyway upon return
    context->srr0 += 4;
}

static void* Func(void* param)
{
    #pragma unused( param )

    OSReport("l: %f\n", 1.0f / Zero);               // Zero division
    return 0;
}

static void Test(OSErrorHandler handler)
{
    OSCreateThread(
        &ThreadA,                           // pointer to the thread to initialize
        Func,                               // pointer to the start routine
        0,                                  // parameter passed to the start routine
        ThreadStackA + sizeof ThreadStackA, // initial stack address
        sizeof ThreadStackA,
        1,                                  // scheduling priority
        0);                                 // joinable by default

    OSSetErrorHandler(OS_ERROR_FPE, handler);

    OSCreateThread(
        &ThreadB,                           // pointer to the thread to initialize
        Func,                               // pointer to the start routine
        0,                                  // parameter passed to the start routine
        ThreadStackB + sizeof ThreadStackB, // initial stack address
        sizeof ThreadStackB,
        1,                                  // scheduling priority
        0);                                 // joinable by default

    OSReport("a: %f\n", LargeNum * LargeNum);       // overflow
    OSReport("b: %f\n", SmallNum * SmallNum);       // underflow
    OSReport("c: %f\n", F / Zero);                  // Zero division

    OSReport("d: %f\n", Inf - Inf);                 // infinity - infinity
    OSReport("e: %f\n", Inf / Inf);                 // infinity / infinity
    OSReport("f: %f\n", Zero / Zero);               // 0 / 0
    OSReport("g: %f\n", Inf * Zero);                // infinity * 0
    OSReport("h: %f\n", Nan < 0);                   // invalid compare

    OSReport("i: %f\n", Snan * Zero);               // signaling NaN

#ifdef __MWERKS__
    OSReport("j: %f\n", __frsqrte(Neg));            // sqrt
#else
    OSReport("j: %f\n", sqrtf(Neg));                // sqrt
#endif
    OSReport("k: %d\n", (int) LargeNum);            // invalid cast

    OSResumeThread(&ThreadA);   // Zero division by thread A
    OSResumeThread(&ThreadB);   // Zero division by thread B
    OSJoinThread(&ThreadA, NULL);
    OSJoinThread(&ThreadB, NULL);
}

void main(void)
{
    Zero     = 0.0f;
    LargeNum = 1.0e30f;
    SmallNum = 1.0e-30f;
    Nan      = Zero / Zero;
    F        = 1.1f;
    Inf      = LargeNum * LargeNum;
    Neg      = -1.0f;
    *(u32*) &Snan = 0xff800001;

    // Run with floating-point error handler
    Test((OSErrorHandler) FloatingPointErrorHandler);
    OSReport("\n");

    // Run without floating-point error handler
    Test((OSErrorHandler) NULL);
    OSReport("\n");

    // Run with Zero division exception only
    __OSFpscrEnableBits = FPSCR_ZE;
    Test((OSErrorHandler) FloatingPointErrorHandler);
    OSReport("\n");

    OSReport("Done.\n");
}

#ifdef  __MWERKS__
#pragma global_optimizer reset
#endif