#include "allnodes.h"
#include "node1.h"
#include "vm.h"
#include "vm_lib.h"
#include SYNTAXFILE	// defined in vm.h: the sintax of the VM
#include "niface.h"

#define CODE(a) Code[a]	//	To access the "Code" OBS Table

/*------------------------------------------------
	Simulate VM program starting at startProgC
	up to maxTim ms. Return the elapsed ms.
	This function is as similar as possible to the
	real IRQ_3 VM function.
	Here I assume that the initial down counter
	value is 2000 (2ms period).
------------------------------------------------*/

void vm (void)  // In OBS is a function called by assembler isr
{
	unsigned int reg,r2,r3;
	unsigned int icode, code;
	static int R[256];
	static int SUbstack[MAXSUBNEST];
	static K_ARGS VM_KS_Args;
	extern STACK_CHAN * K_ArgsP;  // Virtuoso Nano Kernel Stack Channel

		// begin of the sequence
		code = Code[ProgC];
		icode=(CODE(ProgC) & 0xff000000)>>24;

		if(icode & 0x80) {		// CMD
			VMTX(CODE(ProgC));
			ProgC++;
		}
		else {					// RCMD or MTX or NOP
			switch	(icode) {
			case RCMD:
				code=CODE(ProgC);
				reg=code & 0xff;
				code &=0xfff000;
				code <<=6;
				code |=(0xc0000000 |(R[reg] & 0x03ffffff));
				VMTX(code);
				ProgC++;
				break;

			case RSND:						// added on 30/06/2005
				reg=CODE(ProgC)& 0xff;		// R[reg] contain the full command
				VMTX(R[reg]);
				ProgC++;
				break;

			case MTX:
				Irq3_flag = CODE(ProgC) & 0xf;
				ProgC++;
				break;

			case NOP:			// NOP
				ProgC++;
				break;

				// may be none of the above on the first call
			}
		}		// if(icode...
				// execute non command instruction
		while(!(CODE(ProgC) & 0x80000000) &&
			((icode=(CODE(ProgC) & 0xff000000)>>24)>  CMD)) {
			code=CODE(ProgC) &0xffffff;
			switch (icode) {
			case TIM:
				SETTIMER(code);  // macro to switch from simulator to actual code
				break;

			case LTIM:
				SETTIMER(1000*code);  // time in millisec
				break;

			case RTIM:
				SETTIMER(R[code]);	// macro to switch from simulator to actual code
				break;

			case WRT:			// Write R[reg] to DPU HK
//              To be implemented
				break;

			case RINC:			// Increment_Register(reg)
				R[code]++;
				break;

			case RDEC:			// Decrement_Register(reg)
				R[code]--;
				break;

			case RJPR:			// Jmp_Relative_Reg(reg)
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				ProgC +=(R[code]-1);   // always incremented
				break;				// and checked after the switch

			case RSZ:			// Skip_Reg_Zero(reg)
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				if(!R[code]) ProgC++;
				break;

			case RSET:			// Set_Register(reg, val32)
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[code]=CODE(++ProgC);
				break;

			case RADD:			// Add_To_Register(reg, val32)
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[code] +=(int)CODE(++ProgC);
				break;

			case RSUB:			// Subtract_To_Register(reg, val32)
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[code] -=(int)CODE(++ProgC);
				break;

			case RMUL:			// Multiply_To_Register(reg, val32)
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[code] *=(int)CODE(++ProgC);
				break;

			case RDIV:			// Divide_To_Register(reg, val32)
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[code] /=(int)CODE(++ProgC);
				break;

			case RAND:			// And(reg, val32) R[reg] = R[reg] & val32
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[code] &=CODE(++ProgC);
				break;

			case ROR:			// OR(reg, val32) R[reg] = R[reg] | val32
//				   if(code >= MAXREGNO) {
//					   cout<<"Register# too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[code] |=CODE(++ProgC);
				break;

			case RSHR:			// Reg_Shift_Right(reg,val)  R[reg] >>= val
				reg=code >>16;
				code &=0x1f;
				R[reg] >>=code;
				break;

			case RSHL:			// Reg_Shift_Right(reg,val)  R[reg] <<= val
				reg=code >>16;
				code &=0x1f;
				R[reg] <<=code;
				break;

			case CALL:			// Call_Subr(val) ProgC = val
				if(code & 0x800000) code |=0xff000000;	// negative numbers
				SUbstack[++SUbind]=ProgC;    // return address
				ProgC =(code-1);	   // always incremented
				break;				// and checked after the switch

			case JMPR:				// Jmp_Relative(val)
				if(code & 0x800000) code |=0xff000000;	// negative numbers
				ProgC +=(code-1);	   // always incremented
				break;				// and checked after the switch

			case JPNZ:			// JumpNZ(reg, val) If (R[reg] !=0) ProgC = ProgC + val
				reg=code >>16;
				code &=0xffff;
				if(R[reg]) {
					if(code & 0x8000) code |=0xffff0000;	// negative numbers
					ProgC +=(code-1);	   // always incremented
				}
				break;				// and checked after the switch

			case RMOV:				// Move_To_Reg(reg,[Addr]) R[reg]=val32[Addr]
				reg=code >>16;
				code &=0xffff;
				R[reg]=CODE(code);
				break;				// and checked after the switch

			case RRMV:				// Move_To_Reg(reg,[reg1]) R[reg]=val32[R[reg1]]
				reg=code >>16;
				code &=0xff;
				R[reg]=CODE(R[code]);
				break;				// and checked after the switch

			case RSTO:				// // Store_From_Reg(reg,[Addr]) val32[Addr]=R[reg]
				reg=code >>16;
				code &=0xffff;
				CODE(code)=R[reg];
				break;				

			case RRST:				// // Store_From_Reg(reg,[reg1]) val32[R[reg1]]=R[reg]
				reg=code >>16;
				code &=0xff;
				CODE(R[code])=R[reg];
				break;	


			case RREQ:				// Reg_Equate(reg1,reg2)
				reg=code >>16;
				code &=0xffff;
//				   if((code >= MAXREGNO)|| (reg >= MAXREGNO)) {
//					   cout<<"Register # too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				R[reg]=R[code];
				break;

			case XREQ:				// Index_Reg_Equate(reg1,reg2) R(R(reg1))=R(R(reg2))
				reg=R[code >>16];
				code &=0xff;
				code=R[code];
				   if((code >= MAXREGNO)|| (reg >= MAXREGNO)) {
				       ; // runtime errors send event.
				   }
				   else {
				        R[reg]=R[code];
				   }
				break;

			case RSGT:			// Skip_Reg_GT(reg1,reg2)
				reg=code >>16;
				code &=0xffff;
//				   if((code >= MAXREGNO)|| (reg >= MAXREGNO)) {
//					   cout<<"Register # too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				if(R[reg]>R[code]) ProgC++;
				break;

			case RSLT:			// Skip_Reg_LT(reg1,reg2)
				reg=code >>16;
				code &=0xffff;
//				   if((code >= MAXREGNO)|| (reg >= MAXREGNO)) {
//					   cout<<"Register # too high at address "<<ProgC<<endl;
//					   Errors++;
//				   }
				if(R[reg]<R[code]) ProgC++;
				break;

			case RRAD:		// Add_Register_To_Register(r1,r2,r3) R[r1]=R[r2]+R[r3]
				reg=code >>16;
				r2=(code & 0xff00) >>8;
				r3=(code & 0xff);
				R[reg]=R[r2]+R[r3];
				break;

			case RRSB:		// Sub_Register_To_Register(r1,r2,r3) R[r1]=R[r2]-R[r3]
				reg=code >>16;
				r2=(code & 0xff00) >>8;
				r3=(code & 0xff);
				R[reg]=R[r2]-R[r3];
				break;

			case RRMP:		// Mult_Register_To_Register(r1,r2,r3) R[r1]=R[r2]*R[r3]
				reg=code >>16;
				r2=(code & 0xff00) >>8;
				r3=(code & 0xff);
				R[reg]=R[r2]*R[r3];
				break;

			case RRDV:		// Div_Register_To_Register(r1,r2,r3) R[r1]=R[r2]/R[r3]
				reg=code >>16;
				r2=(code & 0xff00) >>8;
				r3=(code & 0xff);
				if(R[r3])		// check for div by 0
					R[reg]=R[r2]/R[r3];
				else {
                    ;       //insert error event
				}
				break;

			case RET:				// Return from subroutine
//				   if(SUbind <0) {
//					   cout<<"Exceeded max RET number at "<<ProgC<<endl;
//					   SUbind++;
//					   Errors++;
//				   }
				ProgC=SUbstack[SUbind];
				SUbind--;
				break;

			case EVNT:			// End current VM program
				break;

			case SVEV:			// Set Virtuoso EVENT
				code &= 0xff;
				VM_KS_Args.Srce = 0;                          // signal comes from node 0
				VM_KS_Args.Comm = EVENTSIGNAL;                // signal means EVENT SIGNAL
				VM_KS_Args.Args.e1.event = code;              // signal directed to event <code>
				VM_KS_Args.Args.e1.opt = 0;                   // no waiting arguments <nino-thesis>
				VM_KS_Args.Args.e1.func = (K_HANDLER *) NULL;  // no handler arguments <nino-thesis>
				prhi_stack_pshC(K_ArgsP, (unsigned int) &VM_KS_Args);
				break;

			case RSVEV:			// Set Virtuoso EVENT by reg  30/06/2005 
				reg &= 0xff;
				code = R[reg];
				VM_KS_Args.Srce = 0;                          // signal comes from node 0
				VM_KS_Args.Comm = EVENTSIGNAL;                // signal means EVENT SIGNAL
				VM_KS_Args.Args.e1.event = code;              // signal directed to event <code>
				VM_KS_Args.Args.e1.opt = 0;                   // no waiting arguments <nino-thesis>
				VM_KS_Args.Args.e1.func = (K_HANDLER *) NULL;  // no handler arguments <nino-thesis>
				prhi_stack_pshC(K_ArgsP, (unsigned int) &VM_KS_Args);
				break;

			case END:			// End current VM program
//				   cout<<"Found END. Normal end of execution"<<endl;
//				   list<<"Found END. Normal end of execution"<<endl;
				Irq3_flag = FALSE; // Frees the LSS Port
				EndProg = TRUE;    // This communcate to the outside world that VM stopped
//				   return now;
				// Stop Timer:
				* P_hw_timer_ctrl = 0x1; // Stop counting, disable IRQ, Reset Timer
				return;
				break;
			}		// switch
			ProgC++;
			if(ProgC > MAXPC) {
//				   ProgC = 8;
				;
			}
		}	// inner while
}