/*
	reg.c
	register display/modify command

*/
#include	"defs.h"

static char	showint,showfloat,showspec;

__attribute__( ( naked ) )
unsigned long getApsrReg( void )
{
	__asm volatile( "mrs 	r0,APSR \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) 
unsigned long getPsrReg( void )
{
	__asm volatile( "mrs 	r0,PSR \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) )
unsigned long getEpsrReg( void )
{
	__asm volatile( "mrs 	r0,EPSR \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) 
unsigned long getEapsrReg( void )
{
	__asm volatile( "mrs 	r0,EAPSR \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) 
unsigned long getMSPReg( void )
{
	__asm volatile( "mrs 	r0,MSP \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) 
unsigned long getPSPReg( void )
{
	__asm volatile( "mrs 	r0,PSP \n");
	__asm volatile( "bx		lr \n");
}

__attribute__(  (naked)  ) unsigned long getControlReg( void ) 
{
	__asm volatile( "mrs 	r0,CONTROL \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) void setControlReg( unsigned long x ) 
{
 	__asm volatile( "msr 	CONTROL,r0 \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) unsigned long getBasepri_MaxReg( void ) 
{
 	__asm volatile( "mrs 	r0,BASEPRI_MAX \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) unsigned long getPrimaskReg( void ) 
{
 	__asm volatile( "mrs 	r0,primask \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) unsigned long getFaultmaskReg( void ) 
{
 	__asm volatile( "mrs 	r0,faultmask \n");
	__asm volatile( "bx		lr \n");
}

__attribute__( ( naked ) ) unsigned long getBasepriReg( void ) 
{
 	__asm volatile( "mrs 	r0,basepri \n");
	__asm volatile( "bx		lr \n");
}


__attribute__( ( naked ) ) void setBasepriReg( unsigned long x ) 
{
 	__asm volatile( "msr 	basepri,r0 \n");
	__asm volatile( "bx		lr \n");
}

void	inireg( void )
{
	showint=1;
	showfloat = 0;
	showspec = 0;;

	alregs.pc = 	0x20000000;
	alregs.sp = 	0x2001C000;
	alregs.apsr = 	0x01000000;
	alregs.lr = 0xFFFFFFFF;

    fpregs.fpscr = 	0xC0000000;

#ifdef MONITOR_DEBUG	
	alregs.r0 = 0;	alregs.r1 = 0;	alregs.r2 = 0;	alregs.r3 = 0;	alregs.r4 = 0;	alregs.r5 = 0;
	alregs.r6 = 0;	alregs.r7 = 0;	alregs.r8 = 0;	alregs.r9 = 0;	alregs.r10 = 0;	alregs.r11 = 0;
	alregs.r12 = 0;
	
	
	fpregs.s0 = 13;	fpregs.s1 = 14;	fpregs.s2 = 15;	fpregs.s3 = 16;	fpregs.s4 = 17;	fpregs.s5 = 18;
	fpregs.s6 = 19;	fpregs.s7 = 20;	fpregs.s8 = 21;	fpregs.s9 = 22;	fpregs.s10 = 23;	fpregs.s11 = 24;
	fpregs.s12 = 25;	fpregs.s13 = 26;	fpregs.s14 = 27;	fpregs.s15 = 28;	fpregs.s16 = 29;
	fpregs.s17 = 30;	fpregs.s18 = 31;	fpregs.s19 = 32;	fpregs.s20 = 33;	fpregs.s21 = 34;
	fpregs.s22 = 35;	fpregs.s23 = 36;	fpregs.s24 = 37;	fpregs.s25 = 38;	fpregs.s26 = 39;
	fpregs.s27 = 40;	fpregs.s28 = 41;	fpregs.s29 = 42;	fpregs.s30 = 43;	fpregs.s31 = 44;
#else
	alregs.r0 = 0;	alregs.r1 = 0;	alregs.r2 = 0;	alregs.r3 = 0;
	alregs.r4 = 0;	alregs.r5 = 0;	alregs.r6 = 0;	alregs.r7 = 0;
	alregs.r8 = 0;	alregs.r9 = 0;	alregs.r10 = 0;	alregs.r11 = 0;
	alregs.r12 = 0;
	
    fpregs.s0 = 0;	fpregs.s1 = 0;	fpregs.s2 = 0;	fpregs.s3 = 0;
	fpregs.s4 = 0;	fpregs.s5 = 0;	fpregs.s6 = 0;	fpregs.s7 = 0;
	fpregs.s8 = 0;	fpregs.s9 = 0;	fpregs.s10 = 0;	fpregs.s11 = 0;
	fpregs.s12 = 0;	fpregs.s13 = 0;	fpregs.s14 = 0;	fpregs.s15 = 0;
	fpregs.s16 = 0;	fpregs.s17 = 0;	fpregs.s18 = 0;	fpregs.s19 = 0;
	fpregs.s20 = 0; fpregs.s21 = 0; fpregs.s22 = 0; fpregs.s23 = 0;
	fpregs.s24 = 0;	fpregs.s25 = 0;	fpregs.s26 = 0;	fpregs.s27 = 0;
	fpregs.s28 = 0;	fpregs.s29 = 0;	fpregs.s30 = 0;	fpregs.s31 = 0;
#endif
}

void	printgen(void)
{
    smallprintf("\nR0 =%X R1 =%X R2 =%X R3 =%X R4 =%X R5 =%X R6 =%X R7 =%X",alregs.r0,alregs.r1,alregs.r2,alregs.r3,alregs.r4,alregs.r5,alregs.r6,alregs.r7);
    smallprintf("\nR8 =%X R9 =%X R10=%X R11=%X R12=%X    APSR=%X    LR(R14)=%X",alregs.r8,alregs.r9,alregs.r10,alregs.r11,alregs.r12, alregs.apsr, alregs.lr);
	smallprintf("\nSP(R13)=%X   PC(R15)=%X " ,alregs.sp ,alregs.pc);  
}

void printfloat( void )
{
	// Print register contents to console
	smallprintf("\nS0 =%X S1 =%X S2 =%X S3 =%X",fpregs.s0,fpregs.s1,fpregs.s2,fpregs.s3);
    smallprintf(" S4 =%X S5 =%X S6 =%X S7 =%X",fpregs.s4,fpregs.s5,fpregs.s6,fpregs.s7);
    smallprintf("\nS8 =%X S9 =%X S10=%X S11=%X",fpregs.s8,fpregs.s9,fpregs.s10,fpregs.s11);
    smallprintf(" S12=%X S13=%X S14=%X S15=%X",fpregs.s12,fpregs.s13,fpregs.s14,fpregs.s15);

    smallprintf("\nS16=%X S17=%X S18=%X S19=%X",fpregs.s16,fpregs.s17,fpregs.s18,fpregs.s19);
    smallprintf(" S20=%X S21=%X S22=%X S23=%X",fpregs.s20,fpregs.s21,fpregs.s22,fpregs.s23);
	smallprintf("\nS24=%X S25=%X S26=%X S27=%X",fpregs.s24,fpregs.s25,fpregs.s26,fpregs.s27);
	smallprintf(" S28=%X S29=%X S30=%X S31=%X",fpregs.s28,fpregs.s29,fpregs.s30,fpregs.s31);
	smallprintf( "\nFPSCR=%X", fpregs.fpscr );
}

void printspec( void )
{
	smallprintf( "\nMSP=%X", getMSPReg() );
	smallprintf( " PSP=%X",  getPSPReg()  );
	smallprintf( " CONTROL=%X",  getControlReg()  );
	smallprintf( " PRIMASK=%d FAULTMASK=%d BASEPRI=%d", getPrimaskReg(),getFaultmaskReg(),getBasepriReg() );
	
}



void printregs( void )
{
	// Print register contents to console
	if( showspec ){
		printspec();
	}		
//	if( showfloat || (getControlReg() & 4) ) {
	if( showfloat ) {
		printfloat();
	}
	if( showint ){
		printgen();
	}
}


void printallregs( void )
{
	// Unconditionally Print register contents to console
	printspec();
	smallprintf( "\nEPSR=%X  EAPSR=%X", getEpsrReg() , getEapsrReg() );
	printfloat();
	printgen();
}



void	printreg( char *name )
{
	_outchar( '\n' ); _outchar( '\r' );

	if( !strcmpi( name , "APSR" ) )
	{
		smallprintf("APSR=%b", alregs.apsr); 
	}
	else if( ( !strcmpi( name , "SP" ) )||( !strcmpi( name , "R13" )) )
	{
		smallprintf("SP=%X", alregs.sp); 
	}
	else if( ( !strcmpi( name , "PC" ) )||( !strcmpi( name , "R15" )) )
	{
		smallprintf("PC=%X", alregs.pc); 
	}
	else if( ( !strcmpi( name , "LR" ) )||( !strcmpi( name , "R14" )) )
	{
		smallprintf("LR=%X", alregs.lr); 
	}
	
	
	
	// data registers
	else if( !strcmpi( name , "R0" ) )
	{
		smallprintf("R0=%X", alregs.r0); 
	}
	else if( !strcmpi( name , "R1" ) )
	{
		smallprintf("R1=%X", alregs.r1); 
	}
	else if( !strcmpi( name , "R2" ) )
	{
		smallprintf("R2=%X", alregs.r2); 
	}
	else if( !strcmpi( name , "R3" ) )
	{
		smallprintf("R3=%X", alregs.r3); 
	}
	else if( !strcmpi( name , "R4" ) )
	{
		smallprintf("R4=%X", alregs.r4); 
	}
	else if( !strcmpi( name , "R5" ) )
	{
		smallprintf("R5=%X", alregs.r5); 
	}
	else if( !strcmpi( name , "R6" ) )
	{
		smallprintf("R6=%X", alregs.r6); 
	}
	else if( !strcmpi( name , "R7" ) )
	{
		smallprintf("R7=%X", alregs.r7); 
	}
	// the address registers
	else if( !strcmpi( name , "R8" ) )
	{
		smallprintf("R8=%X", alregs.r8); 
	}
	else if( !strcmpi( name , "R9" ) )
	{
		smallprintf("R9=%X", alregs.r9); 
	}
	else if( !strcmpi( name , "R10" ) )
	{
		smallprintf("R10=%X", alregs.r10); 
	}
	else if( !strcmpi( name , "R11" ) )
	{
		smallprintf("R11=%X", alregs.r11); 
	}
	else if( !strcmpi( name , "R12" ) )
	{
		smallprintf("R12=%X", alregs.r12); 
	}
}

void	setreg( char *name, int value )
{

	if( !strcmpi( name , "PSR" ) )
	{
		alregs.apsr = value;
	}else if( !strcmpi( name , "PC" ) )
	{
		alregs.pc = value;
	}
	else if( !strcmpi( name , "SP" ) )
	{
		alregs.sp = value;
	}
	else if( !strcmpi( name , "LR" ) )
	{
		alregs.lr = value;
	}

	// data registers
	else if( !strcmpi( name , "R0" ) )
	{
		alregs.r0 = value;
	}
	else if( !strcmpi( name , "R1" ) )
	{
		alregs.r1 = value;
	}
	else if( !strcmpi( name , "R2" ) )
	{
		alregs.r2 = value;
	}
	else if( !strcmpi( name , "R3" ) )
	{
		alregs.r3 = value;
	}
	else if( !strcmpi( name , "R4" ) )
	{
		alregs.r4 = value;
	}
	else if( !strcmpi( name , "R5" ) )
	{
		alregs.r5 = value;
	}
	else if( !strcmpi( name , "R6" ) )
	{
		alregs.r6 = value;
	}
	else if( !strcmpi( name , "R7" ) )
	{
		alregs.r7 = value;
	}
	// the address registers
	else if( !strcmpi( name , "R8" ) )
	{
		alregs.r8 = value;
	}
	else if( !strcmpi( name , "R9" ) )
	{
		alregs.r9 = value;
	}
	else if( !strcmpi( name , "R10" ) )
	{
		alregs.r10 = value;
	}
	else if( !strcmpi( name , "R11" ) )
	{
		alregs.r11 = value;
	}
	else if( !strcmpi( name , "R12" ) )
	{
		alregs.r12 = value;
	}
	
	// floating point registers
	else if( !strcmpi( name , "S0" ) )
	{		fpregs.s0 = value;	}
	else if( !strcmpi( name , "S1" ) )
	{		fpregs.s1 = value;	}
	else if( !strcmpi( name , "S2" ) )
	{		fpregs.s2 = value;	}
	else if( !strcmpi( name , "S3" ) )
	{		fpregs.s3 = value;	}
	else if( !strcmpi( name , "S4" ) )
	{		fpregs.s4 = value;	}
	else if( !strcmpi( name , "S5" ) )
	{		fpregs.s5 = value;	}
	else if( !strcmpi( name , "S6" ) )
	{		fpregs.s6 = value;	}
	else if( !strcmpi( name , "S70" ) )
	{		fpregs.s7 = value;	}
	else if( !strcmpi( name , "S8" ) )
	{		fpregs.s8 = value;	}
	else if( !strcmpi( name , "S9" ) )
	{		fpregs.s9 = value;	}
	else if( !strcmpi( name , "S10" ) )
	{		fpregs.s10 = value;	}
	else if( !strcmpi( name , "S11" ) )
	{		fpregs.s11 = value;	}
	else if( !strcmpi( name , "S12" ) )
	{		fpregs.s12 = value;	}
	else if( !strcmpi( name , "S13" ) )
	{		fpregs.s13 = value;	}
	else if( !strcmpi( name , "S14" ) )
	{		fpregs.s14 = value;	}
	else if( !strcmpi( name , "S15" ) )
	{		fpregs.s15 = value;	}
	else if( !strcmpi( name , "S16" ) )
	{		fpregs.s16 = value;	}
	else if( !strcmpi( name , "S17" ) )
	{		fpregs.s17 = value;	}
	else if( !strcmpi( name , "S18" ) )
	{		fpregs.s18 = value;	}
	else if( !strcmpi( name , "S19" ) )
	{		fpregs.s19 = value;	}
	else if( !strcmpi( name , "S20" ) )
	{		fpregs.s20 = value;	}
	else if( !strcmpi( name , "S21" ) )
	{		fpregs.s21 = value;	}
	else if( !strcmpi( name , "S22" ) )
	{		fpregs.s22 = value;	}
	else if( !strcmpi( name , "S23" ) )
	{		fpregs.s23 = value;	}
	else if( !strcmpi( name , "S24" ) )
	{		fpregs.s24 = value;	}
	else if( !strcmpi( name , "S25" ) )
	{		fpregs.s25 = value;	}
	else if( !strcmpi( name , "S26" ) )
	{		fpregs.s26 = value;	}
	else if( !strcmpi( name , "S27" ) )
	{		fpregs.s27 = value;	}
	else if( !strcmpi( name , "S28" ) )
	{		fpregs.s28 = value;	}
	else if( !strcmpi( name , "S29" ) )
	{		fpregs.s29 = value;	}
	else if( !strcmpi( name , "S30" ) )
	{		fpregs.s30 = value;	}
	else if( !strcmpi( name , "S31" ) )
	{		fpregs.s31 = value;	}
}




void reg( int argc, char ** argv)
{
	//	Forms
	// 	reg 			display registers
	// 	reg NAME		display register value
	//	reg NAME VALUE	set register value
	//	reg show all	display all regs
	//	reg show int	display general regs
	//	reg show float	display floating point regs
	//	reg show special	display special point regs	
	//	reg hide int	hide general regs
	//	reg hide all	hide akk regs
	//	reg hide float	display floating point regs
	//	reg hide spec	display special  regs
	
	int	value;

	if( argc == 3 )
	{
		if( !strcmpi( argv[1], "show" ) )
		{
			if( !strcmpi( argv[2], "int" ) )
			{
				showint=1;
				return;
			}else if( !strcmpi( argv[2], "float" ) )
			{
				showfloat=1;
			}else if( !strcmpi( argv[2], "spec" ) )
			{
				showspec=1;
			}else if( !strcmpi( argv[2], "all" ) )
			{
				showspec=1;
				showfloat=1;
				showint=1;
			}else
				goto gethelp;
		}else if ( !strcmpi( argv[1], "hide" ) )
		{
			if( !strcmpi( argv[2], "int" ) )
			{
				showint=0;
			}else if( !strcmpi( argv[2], "float" ) )
			{
				showfloat=0;
			}else if( !strcmpi( argv[2], "spec" ) )
			{
				showspec=0;
			}else if( !strcmpi( argv[2], "all" ) )
			{
				showspec=0;
				showfloat=0;
				showint=0;
			}else
				goto gethelp;
			
		}else{
			value = decval( argv[2] , 1 );
			setreg( argv[1], value );
			printreg( argv[1] );
			return;
		}
		printregs();
		return;
	}else if( argc == 2 )
	{
		printreg( argv[1] );
		return;
	}else if( argc == 1 )
	{
		printregs();
		return;
	}
gethelp:
	help_reg();
}