/*
** Floating point operators test program
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

typedef struct fpCCR {
    int _exception_bits;	
    int _trap_enable_bits;	
    int _sticky_bits;
    int _rounding_mode;
    unsigned int _unused :16;
    unsigned int _op2	:3;
    unsigned int _dst	:3;
    unsigned int _op1	:3;
    unsigned int _last_operation : 7;
    union {
	float sf;
	double df;
	long double xf;
    } _operand1;
    union {
	float sf;
	double df;
	long double xf;
    } _operand2;
    union {
	int si;
	float sf;
	double df;
	long double xf;
    } _result;
} fpCCR_t;

#define INEXACT 1
#define UNDER	2
#define OVER	4
#define	DIV0	8
#define INVOP	16

#define ROUND_TO_NEAREST	0 // round result to nearest representable value
#define ROUND_TO_ZERO		1 // round result towards zero
#define ROUND_TO_PLUS		2 // round result towards plus infinity
#define ROUND_TO_MINUS		3 // round result towards minus infinity

extern fpCCR_t _fpCCR;

extern void ran_init(unsigned init);
extern unsigned ran_nxt(void);

static double dftype(int nxt);
static float sftype(int nxt);

static void __attribute__ ((interrupt)) fptrap(void);

static int trapped;

#define MAX_TYPES 16

int main(void) {
    char inps[132];
    double da,db,dr;
    float sa,sb,sr;
    int s;
    int i,j,k;
    int *dav,*dbv,*sav,*sbv,*drv,*srv;
    char *op;
    
    // Enable the 5206e instruction cache:
    __asm__ __volatile__("movec %0,%/cacr" : : "d" (0x81000103));
    
    printf("Floating point operator test\n");
    
    dav = (int *)&da;
    dbv = (int *)&db;
    drv = (int *)&dr;
    sav = (int *)&sa;
    sbv = (int *)&sb;
    srv = (int *)&sr;
	
    ran_init(0);
    
//    _fpCCR._trap_enable_bits = INEXACT | UNDER | OVER | DIV0 | INVOP;
    _fpCCR._rounding_mode = ROUND_TO_NEAREST;
    
    // Set vector for TRAP #14
//    *(int *)(0x300000b8) = (int)fptrap;
    
    for (j = 0; j < MAX_TYPES; j += 1) {
	for (k = 0; k < MAX_TYPES; k += 1) {
	    da = dftype(j);
	    db = dftype(k);
	    for (i = 0; i < 4; i += 1) {
		_fpCCR._sticky_bits = 0;
		trapped = 0;
		switch (i) {
		case 0:
		    dr = da + db;
		    op = "+";
		    break;
		case 1:
		    dr = da - db;
		    op = "-";
		    break;
		case 2:
		    dr = da * db;
		    op = "*";
		    break;
		default:
		case 3:
		    dr = da / db;
		    op = "/";
		    break;
		}
		s = _fpCCR._sticky_bits;
		printf("%08x%08x %s %08x%08x = %08x%08x   %c%c%c%c%c %c"
		    "   (%e %s %e = %e)\n",
		    dav[0],dav[1],op,dbv[0],dbv[1],drv[0],drv[1],(s&INVOP)?'I':'i',
		    (s&DIV0)?'D':'d',(s&OVER)?'O':'o',
		    (s&UNDER)?'U':'u',(s&INEXACT)?'X':'x',(trapped)?'T':'t',
		    da,op,db,dr);
	    }
	    gets(inps);
	}
    }
    
    for (j = 0; j < MAX_TYPES; j += 1) {
	for (k = 0; k < MAX_TYPES; k += 1) {
	    sa = sftype(j);
	    sb = sftype(k);
	    for (i = 0; i < 4; i += 1) {
		_fpCCR._sticky_bits = 0;
		trapped = 0;
		switch (i) {
		case 0:
		    sr = sa + sb;
		    op = "+";
		    break;
		case 1:
		    sr = sa - sb;
		    op = "-";
		    break;
		case 2:
		    sr = sa * sb;
		    op = "*";
		    break;
		default:
		case 3:
		    sr = sa / sb;
		    op = "/";
		    break;
		}
		s = _fpCCR._sticky_bits;
		printf("%08x %s %08x = %08x   %c%c%c%c%c %c"
		    "   (%e %s %e = %e)\n",
		    sav[0],op,sbv[0],srv[0],(s&INVOP)?'I':'i',
		    (s&DIV0)?'D':'d',(s&OVER)?'O':'o',
		    (s&UNDER)?'U':'u',(s&INEXACT)?'X':'x',(trapped)?'T':'t',
		    sa,op,sb,sr);
	    }
	    gets(inps);
	}
    }
    
    printf("Done!\n");
    return(0);
}

/*
** Generate various types of doubles
*/
static double dftype(int nxt) {
    double q;
    int exp;
    unsigned u,l;
    int sgn = nxt & 1;
    
    switch (nxt >> 1) {
    case 0:			// Normal finites
	exp = ((2046 * (ran_nxt() >> 12)) >> 20) + 1;
	u = (ran_nxt() & 0x000fffff) | (exp << 20);
	l = ran_nxt();
	break;
	
    case 1:			// Signed zero
	u = 0;
	l = 0;
	break;
	
    case 2:			// Denorm
	do {
	    exp = (52 * (ran_nxt() >> 6)) >> 26;
	    u = 0;
	    if (exp >= 20) {
		l = ran_nxt() >> (exp - 20);
	    } else {
		l = ran_nxt();
		u = ran_nxt() >> (exp + 12);
	    }
	} while (!l && !u);
	break;
	
    case 3:			// Power of 2
	exp = ((2046 * (ran_nxt() >> 12)) >> 20) + 1;
	u = exp << 20;
	l = 0;
	break;
	
    case 4:			// Signed max
	u = 0x7fefffff;
	l = 0xffffffff;
	break;
	
    case 5:			// Signed infinity
	u = 0x7ff00000;
	l = 0;
	break;
	
    case 6:			// Quiet NaN
	u = (ran_nxt() >> 1) | 0x7ff80000;
	l = ran_nxt();
	break;
	
    case 7:			// Signaling NaN
	u = (ran_nxt() >> 13) | 0x7ff00000;
	l = ran_nxt();
	break;
	
    default:
	u = l = 0;
	break;
    }
    
    u |= sgn << 31;
    
    *(int *)&q = u;
    *((int *)&q + 1) = l;
    return(q);
}

/*
** Generate various types of single floats
*/
static float sftype(int nxt) {
    float q;
    int exp;
    unsigned u;
    int sgn = nxt & 1;
    
    switch (nxt >> 1) {
    case 0:			// Normal finites
	exp = ((254 * (ran_nxt() >> 12)) >> 20) + 1;
	u = (ran_nxt() & 0x000fffff) | (exp << 23);
	break;
	
    case 1:			// Signed zero
	u = 0;
	break;
	
    case 2:			// Denorm
	do {
	    exp = (23 * (ran_nxt() >> 6)) >> 26;
	    u = ran_nxt() >> (exp + 9);
	} while (!u);
	break;
	
    case 3:			// Power of 2
	exp = ((254 * (ran_nxt() >> 12)) >> 20) + 1;
	u = exp << 23;
	break;
	
    case 4:			// Signed max
	u = 0x7f7fffff;
	break;
	
    case 5:			// Signed infinity
	u = 0x7f800000;
	break;
	
    case 6:			// Quiet NaN
	u = (ran_nxt() >> 1) | 0x7fc00000;
	break;
	
    case 7:			// Signaling NaN
	u = (ran_nxt() >> 10) | 0x7f800000;
	break;
	
    default:
	u = 0;
	break;
    }
    
    u |= sgn << 31;
    
    *(int *)&q = u;
    return(q);
}

static void __attribute__ ((interrupt)) fptrap(void) {
    trapped = 1;
    return;
}