/*
	dbXX - run.c 
	trace/go commands
*/
#include	"defs.h"

static void	trace_instruction( void );

void		printstack(void );


void	prstat( void )
{	/* Print info about exception status */
	unsigned long status;



	status = SCB->cfsr;
	if( status & (1<<0) ) smallprintf( "\nInstruction access violation" );
	if( status & (1<<1) ) smallprintf( "\nData access violation" );
	if( status & (1<<3) ) smallprintf( "\nMM Unstacking error" );
	if( status & (1<<4) ) smallprintf( "\nMM Stacking error" );
	if( status & (1<<5) ) smallprintf( "\nMM Lazy stacking error" );
	if( status & (1<<7) ) smallprintf( "\nMemory Manage Address Register address valid flag" );
	
	if( status & (1<<8) ) smallprintf( "\nInstruction bus error flag" );
	if( status & (1<<9) ) smallprintf( "\nPrecise data bus error" );
	if( status & (1<<10) ) smallprintf( "\nImprecise data bus error" );
	if( status & (1<<11) ) smallprintf( "\nUnstacking exception return error" );
	if( status & (1<<12) ) smallprintf( "\nStacking exception entry error" );
	if( status & (1<<13) ) smallprintf( "\nLazy stacking error" );
	
	if( status & (1<<15) ) {
		smallprintf( "\nFault address is valid 0x%X", *MMFAR );
		smallprintf( "\nBus Fault address is valid 0x%X", *BFAR );
	}

	if( status & (1<<16) ) 
	{
		smallprintf( "\nUndefined instruction\n" );
		SCB->cfsr |= (1<<16);
	}

	if( status & (1<<17) ) 
	{
		smallprintf( "\nInvalid state, only Thumb-mode supported\n" );
		SCB->cfsr |= (1<<17);
	}
	if( status & (1<<18) ) 
	{
		smallprintf( "\nAttempt to load EXC_RETURN into pc illegally\n" );
		SCB->cfsr |= (1<<18);
	}
	if( status & (1<<19) ) 
	{
		smallprintf( "\nCoprocessor access violation\n" );
		SCB->cfsr |= (1<<19);
	}
	if( status & (1<<24) ) 
	{
		smallprintf( "\nUnaligned memory access\n" );
		SCB->cfsr |= (1<<24);
	}
	if( status & (1<<25) )
	{
		smallprintf( "\nDivide by Zero\n" );
		SCB->cfsr |= (1<<25);
	}

	SCB->cfsr = 0;	/* clear */

	status = SCB->shcsr;
	if( status & (1<<0) ) smallprintf( "\nMemManage is active" );
	if( status & (1<<1) ) smallprintf( "\nBusFault is active" );
	if( status & (1<<3) ) smallprintf( "\nUsageFault is active" );
	if( status & (1<<7) ) smallprintf( "\nSVCall is active" );
	if( status & (1<<8) ) smallprintf( "\nMonitor is active" );
	if( status & (1<<10) ) smallprintf( "\nPendSV is active" );
	if( status & (1<<11) ) smallprintf( "\nSysTick is active" );
	if( status & (1<<12) ) smallprintf( "\nUsage Fault is pended" );
	if( status & (1<<13) ) smallprintf( "\nMem Fault is pended" );
	if( status & (1<<14) ) smallprintf( "\nBus Fault is pended" );
	if( status & (1<<15) ) smallprintf( "\nSVCall is pended" );
	if( status & (1<<16) ) smallprintf( "\nMemManage enable" );
	if( status & (1<<17) ) smallprintf( "\nBus Fault enable" );
	if( status & (1<<18) ) smallprintf( "\nUsageFault enable" );

	status = SCB->hfsr;
	if( status & (1<<1) ) smallprintf( "\nFault occurs because of vector table read on exception processing" );
	if( status & (1<<30) ) smallprintf( "\nHard Fault activated when a configurable Fault was received and cannot activate" );
	if( status & (1<<31) ) smallprintf( "\nFault related to debug" );
	status &= ~(1<<31);	/* must NOT write 1 to this bit */
	SCB->hfsr = status;

	status = SCB->dfsr;
	if( status & 0x01 ) smallprintf( "\nHalt request flag" );
	if( status & 0x02 ) smallprintf( "\nBKPT flag" );
	if( status & 0x04 ) smallprintf( "\nData Watchpoint and Trace (DWT) flag" );
	if( status & 0x08 ) smallprintf( "\nVector catch flag" );
	if( status & 0x10 ) smallprintf( "\nExternal debug request flag" );
	SCB->dfsr = status;
}


void	exception( unsigned long * stack )
{	/* analyze and printout exception */
	smallprintf( "\nException at: 0x%X (SP=%X)", alregs.pc, stack );
	prstat();
	printallregs();
	smallprintf( "\nRESETTING MD407...\n");	
	SCB->aircr = 0x05FA0040;	/* Calm hardware */
	init();
	restart();
}


void Chardfault( unsigned int faulted_stack )
{
	smallprintf( "HARD FAULT with stack: 0x%X", faulted_stack);
	exception( (unsigned long *) faulted_stack ); 
}

void	traced(void)
{
	smallprintf( "\nTRACE:");
	printregs();
	smallprintf( "\n%X ", alregs.pc);
	Instruction( alregs.pc, 1 );
}

void trace( int argc, char ** argv)
{
	int	count;
	int address;
	// tr [address] [instructions]
	count = 1;

	if( argc > 3 )
	{
		help_tr();
		return;
	}

	if( argc >= 2 )
	{	// tr +|-|ADDRESS   		
		if( argv[1][0] ==  '-' )
		{
			hottrace = 0;
			return;
		}else if(argv[1][0] ==  '+' )
		{	
			hottrace = 1;
			return;
		}
		address = decval( argv[1], 1 );
		alregs.pc = ( int ) address;	
	}
	if( argc > 2 )
		count = decval( argv[2], 1 );	


    /* Trace stacking assumes a full stack */
	setControlReg( 4 ); /* Activate FPU */
	while( count > 0 ){
	 	trace_instruction();
		count--;
	}
	// Now print info...
	traced();
}

static	void startapp( unsigned short * address )
{
	if (isbreak( address ) )
	{	/* Starting from a break, always trace one instruction */
	 	trace_instruction();
		/* Now get our new PC */
		if( isbreak( (unsigned short *) alregs.pc ) )
		{
			traced();
			return;
		}
		address = (unsigned short *) alregs.pc;	/* New start address */
	}
	enbreak(( unsigned short *) 0);	/* enable all active breakpoints  */
    *DBG_DEMCR |= MON_EN;
	SCB->shcsr |= 0x70000;		// Enable all traps
    alregs.pc = (unsigned long) address;
   	__asm volatile( "push {r4-r11} \n");
	*DBG_DEMCR |= ( MON_PEND) ;		/* Enter DebugMonitor */
	__asm volatile( "pop {r4-r11} \n");	
    // if we hit a break, we return here sfter DebugMonitor ...
    *DBG_DEMCR &= ~MON_EN;
    disbreak();
	atbreak();
	restart();
}


void go( int argc, char ** argv)
{
	int	hasbreak;
	unsigned long nextaddress;
	unsigned long	*address;
	int	value;
	// go 		run from user PC
	// go -n		step over instruction
	// go ADDRESS	go from address

	if( argc > 2 )
	{
		help_go();
		return;
	}

	if( argc == 2 )
	{	// go -n|ADDRESS 

		if( !strcmpi( argv[1], "-n" ) )
		{
			// determine next sequential instruction
			address = (unsigned long	*) alregs.pc;
			nextaddress = (unsigned long) (address + Instruction( (unsigned long) address,0) );
			hasbreak = isbreak( (unsigned short *) nextaddress );

			if( ! hasbreak )
			{
				// set a break there
				hasbreak = setbp( (unsigned short *) nextaddress, -1 );
				// go...
				startapp( (unsigned short *) address );
				// remove temporary break
				rembp( hasbreak );
			}else{
				// there is already a break there ..
				startapp( (unsigned short *) address );
			}
			return;
		}else
		{
			startapp( (unsigned short *)  decval( argv[1], 1 ) );
			return;
		}		
	}
	startapp( (unsigned short *) alregs.pc );
}




static void	trace_instruction( void )
{
	SCB->shcsr |= 0x70000;		// Enable all traps
	*DBG_DHCSR = (0xA05F<<16);	/* Disable halting debug */
	__asm volatile( "push {r4-r11} \n");
	*DBG_DEMCR |= ( MON_REQ | MON_PEND) ;		/* Enter DebugMonitor */
	__asm volatile( "pop {r4-r11} \n");	
}





#ifdef MONITOR_DEBUG
__attribute__((naked))
void printframe( void )
{
	__asm volatile( "tst lr,#4 \n");
	__asm volatile( "ite eq \n");
	__asm volatile( "mrseq r0,msp \n");	
	__asm volatile( "mrsne r0,psp \n");	
	__asm volatile( "mov r1,lr \n");	
	__asm volatile( "b doprintframe \n");
}

void doprintframe( PContextFrame frame, unsigned long lr )
{
	smallprintf( "\nLink register: 0x%X", lr);

	smallprintf( "\nException frame: 0x%X", frame);
	smallprintf( "\nAt R0: 0x%X:", frame->r0);
	smallprintf( "\nAt R1: 0x%X:", frame->r1);
	smallprintf( "\nAt R2: 0x%X:", frame->r2);
	smallprintf( "\nAt R3: 0x%X:", frame->r3);
	smallprintf( "\nAt R12: 0x%X:", frame->r12);
	smallprintf( "\nAt LR: 0x%X:", frame->lr);
	smallprintf( "\nAt PC_RETURN: 0x%X:", frame->pc_return);
	smallprintf( "\nAt XPSR: 0x%X:", frame->xpsr);
	
	smallprintf( "\nAt S0: 0x%X:", frame->s0);
	smallprintf( "\nAt S1: 0x%X:", frame->s1);
	smallprintf( "\nAt S2: 0x%X:", frame->s2);
	smallprintf( "\nAt S3: 0x%X:", frame->s3);
	smallprintf( "\nAt S4: 0x%X:", frame->s4);
	smallprintf( "\nAt S5: 0x%X:", frame->s5);
	smallprintf( "\nAt S6: 0x%X:", frame->s6);
	smallprintf( "\nAt S7: 0x%X:", frame->s7);
	smallprintf( "\nAt S8: 0x%X:", frame->s8);
	smallprintf( "\nAt S9: 0x%X:", frame->s9);
	smallprintf( "\nAt S10: 0x%X:", frame->s10);
	smallprintf( "\nAt S11: 0x%X:", frame->s11);
	smallprintf( "\nAt S12: 0x%X:", frame->s12);
	smallprintf( "\nAt S13: 0x%X:", frame->s13);
	smallprintf( "\nAt S14: 0x%X:", frame->s14);
	smallprintf( "\nAt S15: 0x%X:", frame->s15);
	smallprintf( "\nAt sfpscr: 0x%X:", frame->fpscr);
	smallprintf( "\nAt Res1: 0x%X:", frame->Res1);
	smallprintf( "\nAt Res2: 0x%X:", frame->Res2);
	
}



void doprintstack( unsigned long stkptr )
{
	smallprintf( "\nCurrent stackpointer: 0x%X", stkptr);
}

__attribute__((naked))
void printstack( void )
{
	__asm volatile( "tst lr,#4 \n");
	__asm volatile( "ite eq \n");
	__asm volatile( "mrseq r0,msp \n");	
	__asm volatile( "mrsne r0,psp \n");	
	__asm volatile( "push {lr} \n");
	__asm volatile( "bl doprintstack \n");
	__asm volatile( "pop {pc} \n");
}

#endif

__attribute__((naked))
void DebugMon_Handler( void )
{
	__asm volatile( "tst lr,#4 \n");
	__asm volatile( "ite eq \n");
	__asm volatile( "mrseq r0,msp \n");	
	__asm volatile( "mrsne r0,psp \n");	
/* 
 * parameter (stack frame ptr)in r0
 * r2,r3 working regs
*/
// Clear the IT-bits in APSR  
	__asm volatile( "ldr	r3,[r0,#28] \n");
	__asm volatile( "and	r3,r3,#0xFFFF07FF \n");	
	__asm volatile( "str	r3,[r0,#28] \n");
    
//	if( *DBG_DEMCR & MON_REQ )
	__asm volatile( "ldr	r3,=0xe000edfc \n");
	__asm volatile( "ldr	r3,[r3] \n");
	__asm volatile( "and.w	r3,r3,#0x80000 \n");	
	__asm volatile( "cmp	r3, #0 \n");
	__asm volatile( "beq.n	L100 \n");

//	{   /* Entering DebugMonitor from 'trace' , Launch single step */
//		*DBG_DEMCR  &= ~( MON_REQ | MON_PEND ) ;			
		__asm volatile( "ldr	r3,=0xe000edfc \n");
		__asm volatile( "ldr	r3,[r3] \n");
		__asm volatile( "ldr	r2,=0xe000edfc \n");
		__asm volatile( "bic.w	r3,r3,#0xa0000 \n");
		__asm volatile( "str	r3, [r2] \n");
			/* Launch single step */
	//		*DBG_DEMCR |= (MON_STEP | MON_EN);
		__asm volatile( "ldr	r3,=0xe000edfc \n");
		__asm volatile( "ldr	r3, [r3, #0] \n");
		__asm volatile( "ldr	r2,=0xe000edfc \n");
		__asm volatile( "orr.w	r3,r3, #0x50000 \n");
		__asm volatile( "str	r3, [r2] \n");
		__asm volatile( "b		RestoreContext \n");	/* Do one instruction */ 
//	}
    __asm volatile( "L100: \n");
//	else if ( *DBG_DFSR & HALTED )
	__asm volatile( "ldr	r3,=0xe000ed30 \n");
	__asm volatile( "ldr	r3, [r3] \n");
	__asm volatile( "and.w	r3,r3,#1 \n");
	__asm volatile( "cmp	r3, #0 \n");
	__asm volatile( "beq.n	L200 \n");
//	{	/* Entering DebugMonitor, after done single step */
//		*DBG_DFSR = HALTED; /* Sticky bit, write 1 to clear */
		__asm volatile( "ldr	r3,=0xe000ed30 \n");
		__asm volatile( "movs	r2, #1 \n");
		__asm volatile( "str	r2, [r3] \n");
//		*DBG_DEMCR &= ~(MON_STEP | MON_EN);
		__asm volatile( "ldr	r3,=0xe000edfc \n");
		__asm volatile( "ldr	r3, [r3] \n");
		__asm volatile( "ldr	r2,=0xe000edfc \n");
		__asm volatile( "bic.w	r3,r3,#0x50000 \n");
		__asm volatile( "str	r3,[r2] \n");
		__asm volatile( "b		SaveContext \n");  /* will return to 'trace' command */
//	}
	__asm volatile( "L200: \n");
//  else if ( *DBG_DFSR & BKPT )
	__asm volatile( "ldr	r3,=0xe000ed30 \n");
	__asm volatile( "ldr	r3,[r3] \n");
	__asm volatile( "and.w	r3,r3,#2 \n");
	__asm volatile( "cmp	r3,#0 \n");
	__asm volatile( "beq.n	L300 \n");
//	{	/* Stopped at breakpoint */ 
//		*DBG_DFSR |= BKPT; /* Sticky bit, write 1 to clear */
        __asm volatile( "ldr	r2,=0xe000ed30 \n");
        __asm volatile( "mov	r3,#2 \n");
        __asm volatile( "str	r3,[r2] \n");
        __asm volatile( "b		SaveContext \n"); /* r0 parameter, will return to 'go' command */ 
//	}else{
	__asm volatile( "L300: \n");
   		__asm volatile( "ldr	r3,=0xe000edfc \n");
		__asm volatile( "ldr	r3,[r3] \n");
		__asm volatile( "ldr	r2,=0xe000edfc \n");
		__asm volatile( "bic.w	r3,r3,#0x40000 \n");
		__asm volatile( "str	r3,[r2] \n");
        __asm volatile( "b		RestoreContext \n");	/* return to 'go' command */ 
// }
	__asm volatile( "bx	lr \n"); /* Should never reach this point... */
}