/* V810 Emulator
 *
 * Copyright (C) 2006 David Tucker
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

//////////////////////////////////////////////////////////
// CPU routines

#include <string.h>

#include "pcfx.h"
#include "interrupt.h"

#include "v810_opt.h"
#include "v810_cpu.h"
#include "v810_cpuD.h"

////////////////////////////////////////////////////////////
// Globals
uint32 P_REG[32];  // Program registers pr0-pr31
uint32 S_REG[32];  // System registers sr0-sr31
static uint32 PC;         // Program Counter

void meowpc(void)
{
 printf("%08x", PC);

}

const uint8 opcycle[0x50] = {
	0x01,0x01,0x01,0x01,0x01,0x01,0x03,0x01,0x0D,0x26,0x0D,0x24,0x01,0x01,0x01,0x01,
	0x01,0x01,0x01,0x01,0x01,0x01,0x03,0x01,0x0F,0x0A,0x05,0x00,0x01,0x01,0x03,0x00, //EI, HALT, LDSR, STSR, DI, BSTR -- Unknown clocks
	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x03,0x03,0x01,0x01,0x01,0x01,
	0x01,0x01,0x0D,0x01,0x01,0x01,0x00,0x01,0x03,0x03,0x1A,0x05,0x01,0x01,0x00,0x01, //these are based on 16-bit bus!! (should be 32-bit?)
	0x01,0x01,0x01,0x01,0x01,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01
};


#define ferr(...)
#define dtprintf(...) exit(1)

//////////////////////////////////////////////////////

// Reinitialize the defaults in the CPU
void v810_reset() {
    P_REG[0]      =  0x00000000;
    PC            =  0xFFFFFFF0;
    //PC		  =  0x00000000;
    S_REG[ECR]    =  0x0000FFF0;
    S_REG[PSW]    =  0x00008000;
    S_REG[PIR]    =  0x00005346;
    S_REG[TKCW]   =  0x000000E0;
}

uint32 v810_timestamp;
static int32 CycleCounter;

int v810_run(int32 RunCycles)
{
    int lowB, highB, lowB2, highB2;             // up to 4 bytes for instruction (either 16 or 32 bits)
    static int opcode;
	int arg1 = 0;
	int arg2 = 0;
	int arg3 = 0;
    uint32 tmp2;
    int j;
    int flags = 0;
    int64 temp = 0;
    uint64 tempu = 0;
    int val = 0;
    uint32 msb = 0;

    static int lastop,lastclock;

    CycleCounter += RunCycles;

    while(CycleCounter > 0)
    {
	uint32 clocks = 0;

	uint32 pint = PCFXIRQ_Check();
	if(pint)
	 v810_int(pint);

	uint32 tmpop = mem_rword(PC);

//	printf("%08x %08x\n", PC, tmpop);
	lowB = tmpop & 0xFF;
	highB = (tmpop >> 8) & 0xFF;
	lowB2 = (tmpop >> 16) & 0xFF;
	highB2 = (tmpop >> 24) & 0xFF;

        P_REG[0]=0; //Zero the Zero Reg!!!

	if ((opcode >0) && (opcode < 0x50)) { //hooray for instruction cache! (cache only if last opcode was not bad!)
		lastop = opcode;
		lastclock = opcycle[opcode];
	}

       	opcode = highB >> 2;
        if((highB & 0xE0) == 0x80)        // Special opcode format for          
            opcode = (highB >> 1);    // type III instructions.

        if((opcode > 0x4F) || (opcode < 0)) {
            //Error Invalid opcode!
            printf("\n%08lx\t\t%2x %2x  ;Invalid Opcode", PC, lowB, highB);
			return 1;
        }

     //   printf("Hi: %08x %02x, %08x\n", PC, opcode, P_REG[12]);

        clocks += opcycle[opcode];
	v810_timestamp += opcycle[opcode];

	//decode opcode and arguments form packed instruction
        switch(addr_mode[opcode]) {
          case AM_I:       // Do the same Ither way =)
          case AM_II:
            arg1 = (lowB & 0x1F);
            arg2 = (lowB >> 5) + ((highB & 0x3) << 3);
            PC += 2;   // 16 bit instruction
            break;

          case AM_III:
            arg1 = ((highB & 0x1) << 8) + (lowB & 0xFE);
            break;

          case AM_IV:
            arg1 = ((highB & 0x3) << 24) + (lowB << 16) + (highB2 << 8) + lowB2;
            break;

          case AM_V:       
            arg3 = (lowB >> 5) + ((highB & 0x3) << 3);
            arg2 = (lowB & 0x1F);
            arg1 = (highB2 << 8) + lowB2;
            PC += 4;   // 32 bit instruction
            break;

          case AM_VIa:  // Mode6 form1
            arg1 = (highB2 << 8) + lowB2;
            arg2 = (lowB & 0x1F);
            arg3 = (lowB >> 5) + ((highB & 0x3) << 3);
            PC += 4;   // 32 bit instruction
            break;

          case AM_VIb:  // Mode6 form2
            arg1 = (lowB >> 5) + ((highB & 0x3) << 3);
            arg2 = (highB2 << 8) + lowB2;                              //  whats the order??? 2,3,1 or 1,3,2
            arg3 = (lowB & 0x1F);
            PC += 4;   // 32 bit instruction
            break;

          case AM_VII:   // Unhandled
            printf("\n%08lx\t\t%2x %2x %2x %2x", PC, lowB, highB, lowB2, highB2);
            PC +=4; // 32 bit instruction
            break;

          case AM_VIII:  // Unhandled
            printf("\n%08lx\t%\t2x %2x %2x %2x", PC, lowB, highB, lowB2, highB2);
            PC += 4;   // 32 bit instruction
            break;

          case AM_IX:
            arg1 = (lowB & 0x1); // Mode ID, Ignore for now
            PC += 2;   // 16 bit instruction
            break;

          case AM_BSTR:  // Bit String Subopcodes
            printf("\nBitStr");
            arg1 = (lowB >> 5) + ((highB & 0x3) << 3);
            arg2 = (lowB & 0x1F);
            PC += 2;   // 16 bit instruction
            break;

          case AM_FPP:   // Floating Point Subcode
            printf("\nFPU");
            arg1 = (lowB >> 5) + ((highB & 0x3) << 3);
            arg2 = (lowB & 0x1F);
            arg3 = ((highB2 >> 2)&0x3F);
            PC += 4;   // 32 bit instruction
            break;

          case AM_UDEF:  // Invalid opcode.
          default:           // Invalid opcode.
            printf("\n%08lx\t\t%2x %2x  ;Invalid Opcode", PC, lowB, highB);
            PC += 2;                                                
            break;
        }

		//process opcode & set flags
        switch(opcode) {
          case MOV:
            P_REG[arg2] = P_REG[arg1];
            break;

          case ADD:
            flags = 0;
            temp = P_REG[arg2] + P_REG[arg1];
            // Set Flags
            if ((int32)temp == 0) flags = flags | PSW_Z;
            if ((int32)temp & 0x80000000)  flags = flags | PSW_S;
			if (temp < P_REG[arg2]) flags = flags | PSW_CY;
            if(((P_REG[arg2]^(~P_REG[arg1]))&(P_REG[arg2]^temp))&0x80000000) flags = flags | PSW_OV;

            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            P_REG[arg2] = (int32)temp;
            break;

          case SUB:
            flags = 0;
			temp = (int64)((uint64)(P_REG[arg2])-(uint64)(P_REG[arg1]));
            // Set Flags
            if ((int32)temp == 0) flags = flags | PSW_Z;
            if ((int32)temp & 0x80000000)  flags = flags | PSW_S;
            if(((P_REG[arg2]^P_REG[arg1])&(P_REG[arg2]^temp))&0x80000000) flags = flags | PSW_OV;
			if ((uint64)(temp) >> 32) flags = flags | PSW_CY;

            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            P_REG[arg2] = (int32)temp;
            break;

          case CMP:
            flags = 0;
			temp = (int64)((uint64)(P_REG[arg2])-(uint64)(P_REG[arg1]));
            // Set Flags
            if ((int32)temp == 0) flags = flags | PSW_Z;
            if ((int32)temp & 0x80000000)  flags = flags | PSW_S;
            if(((P_REG[arg2]^P_REG[arg1])&(P_REG[arg2]^temp))&0x80000000)
				flags = flags | PSW_OV;
			if ((uint64)(temp) >> 32) flags = flags | PSW_CY;
            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;

          case SHL:
            flags = 0;
            val = P_REG[arg1] & 0x1F;
            // set CY before we destroy the regisrer info....
            if((val != 0)&&(P_REG[arg2] >> (32 - val))&0x01) flags = flags | PSW_CY;
            P_REG[arg2] = P_REG[arg2] << val;
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;
			
          case SHR:
            flags = 0; 
            val = P_REG[arg1] & 0x1F;
            // set CY before we destroy the regisrer info....
			if ((val) && ((P_REG[arg2] >> (val-1))&0x01)) flags = flags | PSW_CY;
            P_REG[arg2] = P_REG[arg2] >> val;
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;

          case JMP:
            PC = (P_REG[arg1] & 0xFFFFFFFE);
            break;

          case SAR:
            flags = 0;
            val = P_REG[arg1] & 0x1F;
            msb = P_REG[arg2] & 0x80000000; // Grab the MSB
			
			//carry is last bit shifted out
			if( (val) && ((P_REG[arg2]>>(val-1))&0x01) )
				flags = flags | PSW_CY;

            for(j = 0; j < val; j++)
                P_REG[arg2] = (P_REG[arg2] >> 1)|msb; // Apend the MSB to the end
            
            // Set Flags
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            if (!P_REG[arg2]) flags = flags | PSW_Z;
            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;

          case MUL:
            flags=0;
            temp = (int64)P_REG[arg1] * (int64)P_REG[arg2];
            P_REG[30]   = (int32)(temp >> 32);
			P_REG[arg2] = (int32)temp;

            // Set Flags
	    if (temp != (int32)temp) flags = flags | PSW_OV;
            if ((int32)temp == 0) flags = flags | PSW_Z;
            if ((int32)temp & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;

            break;

          case DIV:
            flags = 0;
            if((int32)P_REG[arg1] == 0) { // Div by zero error
                // Generate exception!
				v810_exp(8, 0xFF80);
            } else {
                if((P_REG[arg2]==0x80000000)&&(P_REG[arg1]==0xFFFFFFFF)) {
					flags = flags |PSW_OV;
					P_REG[30]=0;
					P_REG[arg2] = 0x80000000;
				} else {
					temp        = (int32)P_REG[arg2] % (int32)P_REG[arg1];
					P_REG[arg2] = (int32)P_REG[arg2] / (int32)P_REG[arg1];
					if (arg2 != 30) P_REG[30] = (int32)temp;
				}

				// Set Flags
				if (P_REG[arg2] == 0) flags = flags | PSW_Z;
				if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
				S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags; 
            }
            break;

          case MULU:
            flags = 0;
            tempu = (uint64)P_REG[arg1] * (uint64)P_REG[arg2];
            P_REG[30]   = (int32)(tempu >> 32);
			P_REG[arg2] = (uint32)tempu;

            // Set Flags
  	    if (tempu != (uint32)tempu) flags = flags | PSW_OV;
            if ((int32)tempu == 0) flags = flags | PSW_Z;
            if ((int32)tempu & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            break;

          case DIVU:
            flags = 0;
            if(P_REG[arg1] == 0) { // Div by zero error
                // Generate exception!
				v810_exp(8, 0xFF80);
            } else {
				temp        = (uint32)P_REG[arg2] % (uint32)P_REG[arg1];
                P_REG[arg2] = (uint32)P_REG[arg2] / (uint32)P_REG[arg1];
				if (arg2 != 30) P_REG[30] = (int32)temp;
                // Set Flags
                if (P_REG[arg2] == 0) flags = flags | PSW_Z;
                if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
                S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            }
            break;

          case OR:
            flags = 0;
            P_REG[arg2] = P_REG[arg1] | P_REG[arg2];
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            break;

          case AND:
            flags = 0;
            P_REG[arg2] = P_REG[arg1] & P_REG[arg2];
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            break;

          case XOR:
            flags = 0;
            P_REG[arg2] = P_REG[arg1] ^ P_REG[arg2];
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            break;

          case NOT:
            flags = 0;
			P_REG[arg2] = ~P_REG[arg1];
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            break;

          case MOV_I:
            P_REG[arg2] = sign_5(arg1);
            break;

          case ADD_I:
            flags = 0;
            temp = P_REG[arg2] + sign_5(arg1);
            // Set Flags
            if ((int32)temp == 0) flags = flags | PSW_Z;
            if ((int32)temp & 0x80000000)  flags = flags | PSW_S;
            if(((P_REG[arg2]^(~sign_5(arg1)))&(P_REG[arg2]^temp))&0x80000000) flags = flags | PSW_OV;
			if (temp < P_REG[arg2]) flags = flags | PSW_CY;

            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            P_REG[arg2] = (uint32)temp;
            break;

          case SETF:
			//SETF may contain bugs
			P_REG[arg2] = 0;
			switch (arg1 & 0x0F) {
				case COND_V:
					if (S_REG[PSW] & PSW_OV) P_REG[arg2] = 1;
					break;
				case COND_C:
					if (S_REG[PSW] & PSW_CY) P_REG[arg2] = 1;
					break;
				case COND_Z:
					if (S_REG[PSW] & PSW_Z) P_REG[arg2] = 1;
					break;
				case COND_NH:
					if (S_REG[PSW] & (PSW_CY|PSW_Z)) P_REG[arg2] = 1;
					break;
				case COND_S:
					if (S_REG[PSW] & PSW_S) P_REG[arg2] = 1;
					break;
				case COND_T:
					P_REG[arg2] = 1;
					break;
				case COND_LT:
					if ((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV))) P_REG[arg2] = 1;
					break;
				case COND_LE:
					if (((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV)))|(S_REG[PSW]&PSW_Z)) P_REG[arg2] = 1;
					break;
				case COND_NV:
					if (!(S_REG[PSW] & PSW_OV)) P_REG[arg2] = 1;
					break;
				case COND_NC:
					if (!(S_REG[PSW] & PSW_CY)) P_REG[arg2] = 1;
					break;
				case COND_NZ:
					if (!(S_REG[PSW] & PSW_Z)) P_REG[arg2] = 1;
					break;
				case COND_H:
					if (!(S_REG[PSW] & (PSW_CY|PSW_Z))) P_REG[arg2] = 1;
					break;
				case COND_NS:
					if (!(S_REG[PSW] & PSW_S)) P_REG[arg2] = 1;
					break;
				case COND_F:
					//always false! do nothing more
					break;
				case COND_GE:
					if (!((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV)))) P_REG[arg2] = 1;
					break;
				case COND_GT:
					if (!(((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV)))|(S_REG[PSW]&PSW_Z))) P_REG[arg2] = 1;
					break;
			}
			break;

          case CMP_I:
            flags = 0;
			temp = (int64)((uint64)(P_REG[arg2])-(uint64)(sign_5(arg1)));

            if ((int32)temp == 0) flags = flags | PSW_Z;
            if ((int32)temp & 0x80000000)  flags = flags | PSW_S;
            if(((P_REG[arg2]^(sign_5(arg1)))&(P_REG[arg2]^temp))&0x80000000) flags = flags | PSW_OV;
			if ((uint64)(temp) >> 32) flags = flags | PSW_CY;

            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;

          case SHL_I:
            flags = 0;
            if((arg1)&&(P_REG[arg2] >> (32 - arg1))&0x01) flags = flags | PSW_CY;
            // set CY before we destroy the regisrer info....
            P_REG[arg2] = P_REG[arg2] << arg1;
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;

          case SHR_I:
            flags = 0;
            if((arg1)&&(P_REG[arg2] >> (arg1-1))&0x01) flags = flags | PSW_CY;
            // set CY before we destroy the regisrer info....
            P_REG[arg2] = P_REG[arg2] >> arg1;
            // Set Flags
            if (P_REG[arg2] == 0) flags = flags | PSW_Z;
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;

		  case EI:
            S_REG[PSW] = S_REG[PSW] & (0xFFFFFFFF - PSW_ID);
            break;

          case SAR_I:
            flags = 0;
            msb = P_REG[arg2] & 0x80000000; // Grab the MSB

			if( (arg1) && ((P_REG[arg2]>>(arg1-1))&0x01) )
				flags = flags | PSW_CY;

            for(j = 0; j < arg1; j++)
				P_REG[arg2] = (P_REG[arg2] >> 1) | msb; //Keep sticking the msb on the end

            // Set Flags
            if (P_REG[arg2] & 0x80000000)  flags = flags | PSW_S;
            if (!P_REG[arg2]) flags = flags | PSW_Z;
            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            break;

          case TRAP:
            printf("\nUnhandled opcode! trap");
            break;

          case LDSR:
            S_REG[(arg1 & 0x1F)] = P_REG[(arg2 & 0x1F)];
            break;

          case STSR:
            P_REG[(arg2 & 0x1F)] = S_REG[(arg1 & 0x1F)];
            break;

          case DI:
            S_REG[PSW] = S_REG[PSW] | PSW_ID;
            break;

          case BV:
            if(S_REG[PSW]&PSW_OV) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BL:
            if(S_REG[PSW]&PSW_CY) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BE:
            if(S_REG[PSW]&PSW_Z) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BNH:
            if((S_REG[PSW]&PSW_Z)||(S_REG[PSW]&PSW_CY)) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BN:
            if(S_REG[PSW]&PSW_S) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BR:
            PC += (sign_9(arg1) & 0xFFFFFFFE);
	    clocks += 2;
	    v810_timestamp += 2;
            break;
          case BLT:
            if((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV))) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BLE:
            if(((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV)))||(S_REG[PSW]&PSW_Z)) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BNV:
            if(!(S_REG[PSW]&PSW_OV)) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BNL:
            if(!(S_REG[PSW]&PSW_CY)) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BNE:
            if((S_REG[PSW]&PSW_Z) == PSW_Z) {
                PC +=2;
            } else {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            }
            break;

          case BH:
            if(!((S_REG[PSW]&PSW_Z)||(S_REG[PSW]&PSW_CY))) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BP:
            if(!(S_REG[PSW] & PSW_S)) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case NOP:
            //Its a NOP do nothing =)
            PC +=2;
            break;

          case BGE:
            if(!((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV)))) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
            }
            break;

          case BGT:
            if(!(((!!(S_REG[PSW]&PSW_S))^(!!(S_REG[PSW]&PSW_OV)))||(S_REG[PSW]&PSW_Z))) {
                PC += (sign_9(arg1) & 0xFFFFFFFE);
		clocks += 2;
		v810_timestamp += 2;
            } else {
                PC +=2;
			}
			break;

          case JR:
            PC += (sign_26(arg1) & 0xFFFFFFFE);
            break;

          case JAL:
            P_REG[31]=PC+4;
            PC += (sign_26(arg1) & 0xFFFFFFFE);
	    //printf("Blah: %08x %08x\n", arg1, PC);
            break;

          case MOVEA:
            P_REG[arg3] = P_REG[arg2] + sign_16(arg1);
            break;

          case ADDI:
            flags = 0;
            temp = P_REG[arg2] + sign_16(arg1);
            // Set Flags
            if ((int32)temp == 0) flags = flags | PSW_Z;
            if ((int32)temp & 0x80000000)  flags = flags | PSW_S;
            if (((P_REG[arg2]^(~sign_16(arg1)))&(P_REG[arg2]^temp))&0x80000000) flags = flags | PSW_OV;
			if (temp < P_REG[arg2]) flags = flags | PSW_CY;

            S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
            P_REG[arg3] = (int32)temp;
            break;

          case ORI:
            flags = 0;
            P_REG[arg3] = arg1 | P_REG[arg2];
            // Set Flags
            if (P_REG[arg3] == 0) flags = flags | PSW_Z;
            if (P_REG[arg3] & 0x80000000)  flags = flags | PSW_S;
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            break;

          case ANDI:
            flags = 0;
            P_REG[arg3] = (arg1 & P_REG[arg2]);
            // Set Flags
            if (P_REG[arg3] == 0) flags = (flags | PSW_Z);
            S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
            break;

          case XORI:
			flags = 0;
			P_REG[arg3] = arg1 ^ P_REG[arg2];
			// Set Flags
			if (P_REG[arg3] == 0) flags = flags | PSW_Z;
			if (P_REG[arg3] & 0x80000000)  flags = flags | PSW_S;
			S_REG[PSW] = (S_REG[PSW] & (0xFFFFFFF0|PSW_CY))|flags;
			break;

          case MOVHI:
            P_REG[arg3] = (arg1 << 16) + P_REG[arg2];
			break;

          case RETI:
            //Return from Trap/Interupt
            if(S_REG[PSW] & PSW_NP) { // Read the FE Reg
                PC = S_REG[FEPC];
                S_REG[PSW] = S_REG[FEPSW];
            } else { 	//Read the EI Reg Interupt
                PC = S_REG[EIPC];
                S_REG[PSW] = S_REG[EIPSW];
            }
            break;

          case HALT:
            printf("\nUnhandled opcode! halt");
            break;

          case LD_B:
			tmp2 = (sign_16(arg1)+P_REG[arg2])&0xFFFFFFFF;
			
			P_REG[arg3] = sign_8(mem_rbyte(tmp2));

			//should be 3 clocks when executed alone, 2 when precedes another LD, or 1
			//when precedes an instruction with many clocks (I'm guessing FP, MUL, DIV, etc)
			if (lastclock < 6) {
				if ((lastop == LD_B) || (lastop == LD_H) || (lastop == LD_W)) { clocks += 1; v810_timestamp += 1; }
				else { clocks += 2; v810_timestamp += 2; }
			}
            break;

          case LD_H:
			tmp2 = (sign_16(arg1)+P_REG[arg2]) & 0xFFFFFFFE;
	            P_REG[arg3] = sign_16(mem_rhword(tmp2));

            break;

          case LD_W:
			tmp2 = (sign_16(arg1)+P_REG[arg2]) & 0xFFFFFFFC;
				P_REG[arg3] = mem_rword(tmp2);
			

			if (lastclock < 6) {
				if ((lastop == LD_B) || (lastop == LD_H) || (lastop == LD_W)) { clocks += 3; v810_timestamp+=3; }
				else { clocks += 4; v810_timestamp+=4; }
			}
            break;

          case ST_B:
            mem_wbyte(sign_16(arg2)+P_REG[arg3],P_REG[arg1]&0xFF);
			//clocks should be 2 clocks when follows another ST
			if (lastop == ST_B) { clocks += 1; v810_timestamp+=1; }
            break;

          case ST_H:
            mem_whword((sign_16(arg2)+P_REG[arg3])&0xFFFFFFFE,P_REG[arg1]&0xFFFF);
			if (lastop == ST_H) { clocks += 1; v810_timestamp += 1; }
            break;

          case ST_W:
			tmp2 = (sign_16(arg2)+P_REG[arg3]) & 0xFFFFFFFC;
		        mem_wword(tmp2,P_REG[arg1]);

			if (lastop == ST_W) { clocks += 3; v810_timestamp += 3; }
            break;

          case IN_B:
            P_REG[arg3] = port_rbyte(sign_16(arg1)+P_REG[arg2]);
            break;

          case IN_H:
            P_REG[arg3] = port_rhword((sign_16(arg1)+P_REG[arg2]) & 0xFFFFFFFE);
            break;

          case CAXI:
            printf("\nUnhandled opcode! caxi");
            break;

          case IN_W:
            P_REG[arg3] = port_rword((sign_16(arg1)+P_REG[arg2]) & 0xFFFFFFFC);
            break;

          case OUT_B:
            port_wbyte(sign_16(arg2)+P_REG[arg3],P_REG[arg1]&0xFF);
			//clocks should be 2 when follows another OUT
			if (lastop == OUT_B) { clocks += 1; v810_timestamp += 1; }
            break;

          case OUT_H:
            port_whword((sign_16(arg2)+P_REG[arg3])&0xFFFFFFFE,P_REG[arg1]&0xFFFF);
			if (lastop == OUT_H) { clocks += 1; v810_timestamp += 1; }
            break;

          case OUT_W:
            port_wword((sign_16(arg2)+P_REG[arg3])&0xFFFFFFFC,P_REG[arg1]);
			if (lastop == OUT_W) { clocks += 3; v810_timestamp += 3; }
            break;

          case FPP:
			fpu_subop(arg3,arg1,arg2);
            break;

          case BSTR:
            bstr_subop(arg2,arg1);    
            break;

          default:
            printf("\n%08lx\t\t%2x %2x  ;Invalid Opcode", PC, lowB, highB);
            break;
        }

     CycleCounter -= clocks;
    }
}


//Bitstring routines, wrapper functions for bitstring instructions!
void get_bitstr(uint32 *str, uint32 src, uint32 srcoff, uint32 len) {
	uint32 i=0,tword,tmp;

if(srcoff!=0)
	i=i;

	memset(str,0,(((len>>5)+1)<<2)); //clear bitstring data ((len/32)+1)*4

	tmp = ((i+srcoff)>>5);
	tword = mem_rword(src+(tmp<<2));
	while (i < len) {
		//if next byte, grab it
		if (((i+srcoff)>>5) != tmp) {
			tmp = ((i+srcoff)>>5);
			tword = mem_rword(src+(tmp<<2));
		}
		str[i>>5] |= (((tword >> ((srcoff+i)&0x1F)) & 1) << (i&0x1F));
		i++;
	}
}

void set_bitstr(uint32 *str, uint32 dst, uint32 dstoff, uint32 len) {
	uint32 i=0,tword,tmp;

if(dstoff!=0)
	i=i;

	tmp = ((i+dstoff)>>5);
	tword = mem_rword(dst+(tmp<<2));
	while (i < len) {
		if (((i+dstoff)>>5) != tmp) {
			tmp = ((i+dstoff)>>5);
			tword = mem_rword(dst+(tmp<<2));
		}
		tword &= (~(1<<((dstoff+i)&0x1F)));
		tword |= (((str[i>>5]>>(i&0x1F))&1)<<((dstoff+i)&0x1F));
		i++;
		if (!((i+dstoff)&0x1F)) mem_wword(dst+(tmp<<2),tword);
	}
	mem_wword(dst+(tmp<<2),tword);
}

/*
//Bitstring routines, wrapper functions for bitstring instructions!
void get_bitstr(uint32 *str, uint32 src, uint32 srcoff, uint32 len) {
	uint32 i=0;
	uint32 lword,hword;
	uint32 tmp;

if(srcoff)
	srcoff = srcoff;

	lword = mem_rword(src);
	src+=4;
	hword = mem_rword(src);
	src+=4;

	//grab one more byte than we need to cover any leftover bits
	len = (len >> 5)+1;

	while (len--) {

		tmp = ((uint64)hword<<(32-srcoff));
		tmp |= (lword>>srcoff);
		str[i++] = tmp;

		//grab next byte
		lword = hword;
		hword = mem_rword(src);
		src+=4;
	}
}

void set_bitstr(uint32 *str, uint32 dst, uint32 dstoff, uint32 len) {
	uint32 i=0;
	uint32 tlow, thigh, tmp;
	uint32 low_mask, high_mask, high_shift;
	uint32 remainder;

	high_shift = 32-dstoff;
	low_mask   = (((uint64)2<<high_shift)-1)>>1;
	high_mask  = ((2<<dstoff)-1)>>1;

	thigh  = mem_rword(dst);
	thigh &= high_mask;

	//remove any bits not divisible by 32, and deal with them later
	len+=dstoff;
	remainder = len%32;
	len       = len>>5;

	while (len--) {
		tlow = str[i] & low_mask;

		tmp  = ((uint64)thigh<<high_shift);
		tmp |= tlow;

		mem_wword(dst, tmp);
		dst+=4;

		//grab next byte
		thigh = str[i] & high_mask;
		i++;
	}

	//hanlde the leftover bits
	if(remainder) {
		tmp = mem_rword(dst);
		//zero out area to write to 
		tmp &= ~((2<<(remainder-1))-1);

		tmp |= (((uint64)thigh<<high_shift))&(((uint64)2<<(remainder-1))-1);
		mem_wword(dst,tmp);
	}
}
*/
/*
//Bitstring routines, wrapper functions for bitstring instructions!
void get_bitstr(uint32 *str, uint32 src, uint32 srcoff, uint32 len) {
	uint32 i=0;
	uint32 lword,hword;
	uint32 tmp;

	lword = mem_rword(src-4);
	hword = mem_rword(src);
	src+=4;

	//grab one more byte than we need to cover any leftover bits
	len = (len >> 5)+1;

	while (len--) {

		tmp = ((uint64)lword<<(32-srcoff));
		tmp |= (hword>>srcoff);
		str[i++] = tmp;

		//grab next byte
		lword = hword;
		hword = mem_rword(src);
		src+=4;
	}
}

void set_bitstr(uint32 *str, uint32 dst, uint32 dstoff, uint32 len) {
	uint32 i=0;
	uint32 tlow, thigh, tmp;
	uint32 low_mask, high_mask, high_shift;
	uint32 remainder;

	high_shift = 32-dstoff;
	low_mask   = (((uint64)2<<high_shift)-1)>>1;
	high_mask  = ((2<<dstoff)-1)>>1;

	thigh  = mem_rword(dst);
	thigh &= high_mask;

	//remove any bits not divisible by 32, and deal with them later
	len+=dstoff;
	remainder = len%32;
	len       = len>>5;

	while (len--) {
		tlow = str[i] & low_mask;

		tmp  = ((uint64)thigh<<high_shift);
		tmp |= tlow;

		mem_wword(dst, tmp);
		dst+=4;

		//grab next byte
		thigh = str[i] & high_mask;
		i++;
	}

	//hanlde the leftover bits
	if(remainder) {
		tmp = mem_rword(dst);
		//zero out area to write to 
		tmp &= ~((2<<(remainder-1))-1);

		tmp |= (((uint64)thigh<<high_shift))&(((uint64)2<<(remainder-1))-1);
		mem_wword(dst,tmp);
	}
}
*/

int bstr_subop(int sub_op, int arg1) {
	uint32 i,tmp[8192],tmp2[8192];

	uint32 dstoff = (P_REG[26] & 0x1F);
	uint32 srcoff = (P_REG[27] & 0x1F);
	uint32 len =     P_REG[28];
	uint32 dst =    (P_REG[29] & 0xFFFFFFFC);
	uint32 src =    (P_REG[30] & 0xFFFFFFFC);


    if(sub_op > 15) {
        printf("\n%08lx\tBSR Error: %04x", PC,sub_op);
		return 0;
    }

    switch(sub_op) {
	case SCH0BSU:
		printf("\nSCH0BSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);
		break;

	case SCH0BSD:
		printf("\nSCH0BSD, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);
		break;

	case SCH1BSU:
		printf("\nSCH1BSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);
		break;

	case SCH1BSD:
		printf("\nSCH1BSD, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);
		break;

	case ORBSU:
		printf("\nORBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		get_bitstr(tmp2,dst,dstoff,len);
		for (i = 0; i < ((len>>5)+1); i++) tmp[i] |= tmp2[i];
		set_bitstr(tmp,dst,dstoff,len);
		break;

	case ANDBSU:
		printf("\nANDBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		get_bitstr(tmp2,dst,dstoff,len);
		for (i = 0; i < ((len>>5)+1); i++) tmp[i] &= tmp2[i];
		set_bitstr(tmp,dst,dstoff,len);
		break;

	case XORBSU:
		printf("\nXORBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		get_bitstr(tmp2,dst,dstoff,len);
		for (i = 0; i < ((len>>5)+1); i++) tmp[i] ^= tmp2[i];
		set_bitstr(tmp,dst,dstoff,len);
		break;

	case MOVBSU:
		printf("\nMOVBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		set_bitstr(tmp,dst,dstoff,len);
		break;

	case ORNBSU:
		printf("\nORNBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		get_bitstr(tmp2,dst,dstoff,len);
		for (i = 0; i < ((len>>5)+1); i++) tmp[i] = (~tmp[i] | tmp2[i]);
		set_bitstr(tmp,dst,dstoff,len);
		break;

	case ANDNBSU:
		printf("\nANDNBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		get_bitstr(tmp2,dst,dstoff,len);
		for (i = 0; i < ((len>>5)+1); i++) tmp[i] = (~tmp[i] & tmp2[i]);
		set_bitstr(tmp,dst,dstoff,len);
		break;

	case XORNBSU:
		printf("\nXORNBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		get_bitstr(tmp2,dst,dstoff,len);
		for (i = 0; i < ((len>>5)+1); i++) tmp[i] = (~tmp[i] ^ tmp2[i]);
		set_bitstr(tmp,dst,dstoff,len);
		break;

	case NOTBSU:
		printf("\nNOTBSU, len: %08X, src: %08X, srcoff: %08X, dst: %08X, dstoff: %08X",len,src,srcoff,dst,dstoff);

		get_bitstr(tmp,src,srcoff,len);
		for (i = 0; i < ((len>>5)+1); i++) tmp[i] = ~tmp[i];
		set_bitstr(tmp,dst,dstoff,len);
		break;

	default:
		break;
	}

	return 0;
}

int fpu_subop(int sub_op, int arg1, int arg2) {
	int i, flags = 0; // Set Flags, OV set to Zero
	double dTemp;
	float fTemp;
	int temp;

    if(sub_op > 15) {
        printf("\n%08lx\tFPU Error: %04x", PC, sub_op);
		return 0;
    }

	switch(sub_op) {
	case CMPF_S:
		dTemp = (double)(*((float *)&P_REG[arg1])) - (double)(*((float *)&P_REG[arg2]));

		if (dTemp == 0.0F) flags = flags | PSW_Z;
		if (dTemp < 0.0F)  flags = flags | PSW_S;
		if (dTemp > ((float)dTemp)) flags = flags | PSW_CY; //How???
		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;
		break;

	case CVT_WS:
		fTemp = (float)((int32)P_REG[arg2]);

		if (fTemp == 0) flags = flags | PSW_Z;
		if (fTemp < 0.0F)  flags = flags | PSW_S;
		if (P_REG[arg2] != fTemp) flags = flags | PSW_CY; //How???
		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;

		P_REG[arg1] = *((uint32 *)&fTemp);
		break;

	case CVT_SW:
		P_REG[arg1] = (int32)(*((float *)&P_REG[arg2])+0.5F);

		if (P_REG[arg1] == 0) flags = flags | PSW_Z;
		if (P_REG[arg1] & 0x80000000)  flags = flags | PSW_S;
		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF7)|flags;
		break;

	case ADDF_S:
		dTemp = (double)(*((float *)&P_REG[arg1])) + (double)(*((float *)&P_REG[arg2]));

		if (dTemp == 0.0F) flags = flags | (PSW_Z | PSW_CY);  //changed by frostgiant based on NEC docs
		if (dTemp < 0.0F)  flags = flags | PSW_S;

		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;

		fTemp = ((float)dTemp);
		P_REG[arg1] = *((uint32 *)&fTemp);
		break;

	case SUBF_S:
		dTemp = (double)(*((float *)&P_REG[arg1])) - (double)(*((float *)&P_REG[arg2]));

		if (dTemp == 0.0F) flags = flags | (PSW_Z | PSW_CY);  //changed by frostgiant based on NEC docs
		if (dTemp < 0.0F)  flags = flags | PSW_S;

		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;

		fTemp = ((float)dTemp);
		P_REG[arg1] = *((uint32 *)&fTemp);
		break;

	case MULF_S:
		dTemp = (double)(*((float *)&P_REG[arg1])) * (double)(*((float *)&P_REG[arg2]));

		if (dTemp == 0.0F) flags = flags | (PSW_Z | PSW_CY);  //changed by frostgiant based on NEC docs
		if (dTemp < 0.0F)  flags = flags | PSW_S;
		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;

		fTemp = ((float)dTemp);
		P_REG[arg1] = *((uint32 *)&fTemp);
		break;

	case DIVF_S:
		dTemp = (double)(*((float *)&P_REG[arg1])) / (double)(*((float *)&P_REG[arg2]));

		if (dTemp == 0.0F) flags = flags | (PSW_Z | PSW_CY);  //changed by frostgiant based on NEC docs
		if (dTemp < 0.0F)  flags = flags | PSW_S;

		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF0)|flags;

		fTemp = ((float)dTemp);
		P_REG[arg1] = *((uint32 *)&fTemp);
		break;

	case XB:
		if (arg2) printf("\nXB Instruction, arg2 = r%d",arg2);
		P_REG[arg1] = ((P_REG[arg1]&0xFFFF0000) | (((P_REG[arg1]<<8)&0xFF00) | ((P_REG[arg1]>>8)&0xFF)));
		break;

	case XH:
		if (arg2) printf("\nXH Instruction, arg2 = r%d",arg2);
		P_REG[arg1] = (P_REG[arg1]<<16)|(P_REG[arg1]>>16);
		break;

	case REV:
		temp = 0;
		for (i = 0; i < 32; i++) temp = ((temp << 1) | ((P_REG[arg2] >> i) & 1));
		P_REG[arg1] = temp;
		break;

	case TRNC_SW:
		P_REG[arg1] = (uint32)(*((float *)&P_REG[arg2])+0.5F);

		if (!P_REG[arg1]) flags = flags | PSW_Z;
		if (P_REG[arg1] & 0x80000000)  flags = flags | PSW_S;
		S_REG[PSW] = (S_REG[PSW] & 0xFFFFFFF7)|flags;
		break;

	case MPYHW:
		//if (P_REG[arg1] & 0xFFFF0000) dtprintf(10,ferr,"\nMPYHW Instruction, arg1 = %08X",P_REG[arg1]);
		//if (P_REG[arg2] & 0xFFFF0000) dtprintf(10,ferr,"\nMPYHW Instruction, arg2 = %08X",P_REG[arg2]);
		P_REG[arg1 & 0x1F] = P_REG[arg1 & 0x1F] * P_REG[arg2 & 0x1F];
		break;

	default:
        	printf("\n%08lx\tFPU Error: %04x", PC, sub_op);
		break;
	}

	return 0;
}


// Generate Interupt #n
void v810_int(uint32 iNum) {
    //printf("\nInt atempt %x",iNum);

    if (iNum > 0x0F) return;  // Invalid Interupt number...
    if((S_REG[PSW] & PSW_NP)) return;
    if((S_REG[PSW] & PSW_EP)) return; // Exception pending?
    if((S_REG[PSW] & PSW_ID)) return; // Interupt disabled
    if(iNum < ((S_REG[PSW] & PSW_IA)>>16)) return; // Interupt to low on the chain

    //dtprintf(6,ferr,"\nInt %x",iNum);

    //Ready to Generate the Interupts
    S_REG[EIPC]  = PC;
    S_REG[EIPSW] = S_REG[PSW];

    PC = 0xFFFFFE00 | (iNum << 4);
    
    S_REG[ECR] = 0xFE00 | (iNum << 4);
    S_REG[PSW] = S_REG[PSW] | PSW_EP;
    S_REG[PSW] = S_REG[PSW] | PSW_ID;
    if((iNum+=1) > 0x0F) 
		(iNum = 0x0F);
    S_REG[PSW] = S_REG[PSW] | (iNum << 16); //Set the Interupt

}


// Generate exception #n
//Exceptions are Div by zero, trap and Invalid Opcode, we can live without...
void v810_exp(uint32 iNum, uint32 eCode) {
    if (iNum > 0x0F) return;  // Invalid Exception number...

    //if(!S_REG[PSW]&PSW_ID) return;
    //if(iNum < ((S_REG[PSW] & PSW_IA)>>16)) return; // Interupt to low on the mask level....
    if ((S_REG[PSW] & PSW_IA)>>16) return; //Interrupt Pending

	eCode &= 0xFFFF;
/*
    if(S_REG[PSW]&PSW_NP) { //Fatal Exception
        S_REG[DPC] = PC;
        S_REG[DPSW] = S_REG[PSW];
        S_REG[PSW] = S_REG[PSW] | PSW_DP;
        S_REG[PSW] = S_REG[PSW] | PSW_NP;
        S_REG[PSW] = S_REG[PSW] | PSW_EP;
        S_REG[PSW] = S_REG[PSW] | PSW_ID;
        //S_REG[PSW] = S_REG[PSW] | (((iNum+1) & 0x0f) << 16); //Set the Interupt status

        PC = 0xFFFFFFE0;
        return;
    }else
*/
    if(S_REG[PSW]&PSW_EP) { //Double Exception
        S_REG[FEPC] = PC;
        S_REG[FEPSW] = S_REG[PSW];
        S_REG[ECR] = (eCode << 16); //Exception Code, dont get it???
        S_REG[PSW] = S_REG[PSW] | PSW_NP;
        S_REG[PSW] = S_REG[PSW] | PSW_ID;
        //S_REG[PSW] = S_REG[PSW] | (((iNum+1) & 0x0f) << 16); //Set the Interupt status

        PC = 0xFFFFFFD0;
        return;
    } else {                                // Regular Exception
        S_REG[EIPC] = PC;
        S_REG[EIPSW] = S_REG[PSW];
        S_REG[ECR] = eCode; //Exception Code, dont get it???
        S_REG[PSW] = S_REG[PSW] | PSW_EP;
        S_REG[PSW] = S_REG[PSW] | PSW_ID;
        //S_REG[PSW] = S_REG[PSW] | (((iNum+1) & 0x0f) << 16); //Set the Interupt status

        PC = 0xFFFFFF00 | (iNum << 4);
        return;
    }
}



