/*  	main.c
 	provide dbXX 'init', 'help' and 'main' 
*/ 

#include 	"defs.h"
#include 	<stdio.h>

/* Globals */
int		hottrace;
unsigned 	char inbuf[SZOFINBUF];
AL_REGS	alregs;		/* application level registers */
FP_REGS	fpregs;		/* floating point registers */


ContextFrame context;

/* 'system_stack' saves monitor stack during go and trace,
	used in run.c
	lowlevel.s
*/
long	system_stack, system_lr;	


/* Monitor serial communication (USART 1 */
void _outchar( unsigned char ch )
{
	USART1->dr = (unsigned short) ch;
 	while (( USART1->sr & 0x80)==0);
}

unsigned char  _inchar( void )
{
	while( (USART1->sr & 0x20)==0);
	return (char) USART1->dr;
}
void uartinit(void )
{

	RCC->apb2enr |= 0x10; 		/* Enable USART 1 clock */
	RCC->ahb1enr |= 1; 			/* Enable port A */
	
	RCC->apb2rstr	|= 0x10; 	/* Reset USART1 */	
	RCC->apb2rstr	&= ~0x10; 	
	RCC->ahb1rstr |= 1; 		/* Reset port A */
	RCC->ahb1rstr &= ~1; 		/* Reset port A */

	/* Connect USART pins to AF */
	GPIOA->afrh |= 0x770;
	/* PA9 - USART1 TX  */
	/* PA10 - USART1 RX */
	GPIOA->moder |= 0x00280000;	/* Pins 9 and 10 are AF */
	GPIOA->pupdr |= 0x00100000;	/* Pin 10 pull up */
	
	/* Now, start the USART */
	USART1->brr = 0x2D9;	/* Set UART baudrate (115 200) */
	USART1->cr1 = 0x200C;	/* Enable USART, TE/RX */
}	




void	init_vectorbase( )
{	/* copy entire vector table from FLASH to RWM */
	int i;
	for( i = 0; i < (16+82); i++)
		g_pfnVectors_copy[i] = g_pfnVectors[i];	
	SCB->vtor = (unsigned long )&g_pfnVectors_copy;
	//SCB_VTOR = (unsigned long )&g_pfnVectors_copy;		/* Set the interrupt vector table position */
}

void	restore_vectorbase( int all )
{	/* copy monitor reserved vectors table from FLASH to RWM */
	int i;
	if( all ){
		for( i = 0; i < 15; i++)
			g_pfnVectors_copy[i] = g_pfnVectors[i];
		
	}else{
		g_pfnVectors_copy[0] = g_pfnVectors[0];
		g_pfnVectors_copy[1] = g_pfnVectors[1];
		g_pfnVectors_copy[2] = g_pfnVectors[2];	/* NMI */
		g_pfnVectors_copy[3] = g_pfnVectors[3];	/* HARD FAULT */
		g_pfnVectors_copy[12] = g_pfnVectors[12]; /* DEBUG MONITOR */
		/* Set exception priorities lower than debug monitor */
	}
	SCB->shpr1 = 0x00E0E0E0;
	SCB->shpr2 = 0xE0000000;
	SCB->shpr3 = 0xE0F00000;	/* Set PendSV lowest priority */
	SCB->vtor = (unsigned long )&g_pfnVectors_copy;
}

/* Enable and init MD407 ports D and E */
void	portinit( void )
{
	RCC->ahb1enr |= 0x18;		/* Enable port D and E clocks */
	RCC->apb2enr |= 0x4000; 	/* Enable SYSCFG clock */

	RCC->ahb1rstr |= 0x18; 		/* Reset port D and E */	
	RCC->ahb1rstr  &= ~0x18; 		
	RCC->ahb2rstr	|= 0x4000; 	/* Reset SYSCFG */
	RCC->ahb2rstr	&= ~0x4000; 	
	
	/* Setup PE0-15 as input */	
    GPIOE->moder = 0;
    GPIOE->pupdr = 0xAAAAAAAA;   /* default pull-down */
    GPIOE->otyper = 0;
    GPIOE->ospeedr = 0;
    GPIOE->idr = 0;
    GPIOE->odr = 0;
		
	/* Setup PD0-15 as input */
    GPIOD->moder = 0;
    GPIOD->pupdr = 0xAAAAAAAA;   /* default pull-down */
    GPIOD->otyper = 0;
    GPIOD->ospeedr = 0;
    GPIOD->idr = 0;
    GPIOD->odr = 0;
}



void	welcome( void ) { 	
	smallprintf("\n *** CTH/ce %s monitor/debugger for %s",DEBUGGER_NAME,DEBUGGER_MACHINE); 	
	smallprintf("\n *** Version:%d.%d", MAJOR_VERSION, MINOR_VERSION ); 
 }
	
void	init( void ) { 
	/* Do HW-init ... */	
	init_vectorbase();
	portinit();
	uartinit();

	SCB->shcsr = 0x70000;		/* Enable all traps */
	SCB->ccr = 0x218;			/* STKALIGN, DIV_0_TRP , UNALIGNED */
//	SCB->ccr = 0x19;			/* STKALIGN, DIV_0_TRP , UNALIGNED */
		
	*FPU_FPCCR &= 0x3FFFFFFF;	/* Disable lazy stacking */
	*FPU_CPACR |= 0x00F00000;	/* Enable the FPU */

	hottrace = 1;	/* Set dgbARM global variables */
	inibp();		/* init breakpoint table if required */
	inireg( );		/* init user registers */
} 


/* Following arameters are for the MD407 (8MHz X-tal) working at 168 MHz */
#define PLL_M      8
#define PLL_N      336
/* SYSCLK = PLL_VCO / PLL_P */
#define PLL_P      2
/* USB OTG FS, SDIO and RNG Clock =  PLL_VCO / PLLQ */
#define PLL_Q      8


static void SetSysClock(void)
{
  volatile uint32_t StartUpCounter = 0, HSEStatus = 0;
  
  RCC->cr |= ((uint32_t) RCC_CR_HSEON);		/* Enable HSE */
 
  do
  { /* Wait till HSE is ready and if Time out is reached exit */
    HSEStatus = RCC->cr & 0x00020000;
    StartUpCounter++;
//	#define HSE_STARTUP_TIMEOUT    ((uint16_t)0x05000)   /*!< Time out for HSE start up */

  } while((HSEStatus == 0) && (StartUpCounter != ((uint16_t)0x05000)));

  if ((RCC->cr & 0x00020000) != 0)
//  if ((RCC->cr & 0x00020000) != RESET)
  {
    HSEStatus = (uint32_t)0x01;
  }
  else
  {
    HSEStatus = (uint32_t)0x00;
  }

  if (HSEStatus == (uint32_t)0x01)
  {
    /* Select regulator voltage output Scale 1 mode, System frequency up to 168 MHz */
    RCC->apb1enr |= RCC_APB1ENR_PWREN;
    PWR->cr |= PWR_CR_VOS;

    RCC->cfgr |= RCC_CFGR_HPRE_DIV1;	/* HCLK = SYSCLK / 1*/
	RCC->cfgr |= RCC_CFGR_PPRE2_DIV2;	/* PCLK2 = HCLK / 2*/
    RCC->cfgr |= RCC_CFGR_PPRE1_DIV4;	/* PCLK1 = HCLK / 4*/

    /* Configure the main PLL */
    RCC->pllcfgr = PLL_M | (PLL_N << 6) | (((PLL_P >> 1) -1) << 16) |
                   (RCC_PLLCFGR_PLLSRC_HSE) | (PLL_Q << 24);

    RCC->cr |= RCC_CR_PLLON;	/* Enable the main PLL */

    while((RCC->cr & RCC_CR_PLLRDY) == 0)	
    {  /* Wait till the main PLL is ready */
    }
   
    /* Configure Flash prefetch, Instruction cache, Data cache and wait state */
    FLASH->acr = FLASH_ACR_ICEN |FLASH_ACR_DCEN |FLASH_ACR_LATENCY_5WS;

    /* Select the main PLL as system clock source */
    RCC->cfgr &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
    RCC->cfgr |= RCC_CFGR_SW_PLL;

    while ((RCC->cfgr & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL);
    {  /* Wait till the main PLL is used as system clock source */
    }
  }
  else
  { /* If HSE fails to start-up, the application will have wrong clock
         configuration.  */
  }

}
void clockinit(void)
{

  /* Reset the RCC clock configuration to the default reset state ------------*/
  /* Set HSION bit */
  RCC->cr |= (uint32_t)0x00000001;

  /* Reset CFGR register */
  RCC->cfgr = 0x00000000;

  /* Reset HSEON, CSSON and PLLON bits */
  RCC->cr &= (uint32_t)0xFEF6FFFF;

  /* Reset PLLCFGR register */
  RCC->pllcfgr = 0x24003010;

  /* Reset HSEBYP bit */
  RCC->cr &= (uint32_t)0xFFFBFFFF;

  /* Disable all interrupts */
  RCC->cir = 0x00000000;

  /* Configure the System clock source, PLL Multiplier and Divider factors, 
     AHB/APBx prescalers and Flash settings ----------------------------------*/
  SetSysClock();

}






/* When GDB starts MD407 it resets the computer.
 * Applications running under the monitor/debugger thus has to initialize the system.
 * 'gdbInit' sets upp MD407 clocks and ports D and E for simple debugging with GDB/USBDM
*/

void	gdbInit( void )
{	
	clockinit();
	portinit();
	SCB->vtor = (unsigned long )&g_pfnVectors_copy;
}



/* ----------------------------------------------------------------------------------- */

 void	help_tr( void ) { 	
 // 	tr	- Trace instruction(s)
//	Forms
// 	tr [address] [instructions] 	
// 	tr + 	
// 	tr - 	
//  	
//    tr<Enter>             Trace one instruction at user PC 	
//    tr ADDRESS            Trace one instruction at ADDRESS 	
//    tr ADDRESS NUMBER     Trace NUMBER instructions at ADDRESS 	
//    tr +                  Turn 'hot trace' on 	
//                          A '.' (dot) at command line will 	
//                          do a 'tr<Enter>' 	
//    tr -                  Turn 'hot trace' off

	smallprintf( "\n tr   - Trace instruction(s)"); 
	smallprintf( "\n tre   - Trace instruction(s), with exception handling"); 
	smallprintf( "\n Forms:"); 

	smallprintf( "\n tr(e) [address] [instructions]"); 
	smallprintf( "\n tr +"); 
	smallprintf( "\n tr -"); 
	smallprintf( "\n\n tr(e)<Enter>	Trace one instruction at user PC"); 
	smallprintf( "\n tr(e) ADDRESS		Trace one instruction at ADDRESS"); 
	smallprintf( "\n tr(e) ADDRESS NUMBER	Trace NUMBER instructions at ADDRESS"); 
	smallprintf( "\n tr +		Turn 'hot trace' on"); 
	smallprintf( "\n 			A '.' (dot) at command line will"); 
	smallprintf( "\n			do a 'tr<Enter>'"); 
	smallprintf( "\n tr -		Turn 'hot trace' off"); 
}  

void	help_go( void ) { 	
// 	go	- Run program
//	Forms
// 	go 		run from user PC 	
// 	go -n		step over instruction 	
// 	go ADDRESS	go from address 		
	smallprintf( "\n go - Run program"); 
	smallprintf( "\n goe - Run program, with exception handling"); 
	smallprintf( "\n Forms:"); 
	smallprintf( "\n go(e) 		run from user PC"); 
	smallprintf( "\n go(e) -n	step over instruction"); 
	smallprintf( "\n go(e) ADDRESS	run from address"); 
}  

void	help_reg( void ) {
//	reg	- Display/Set register values
//	Forms
// 	reg 			display registers
// 	reg NAME		display register value
//	reg NAME VALUE	set register value
//	reg show int	display general regs
//	reg show float	display floating point regs
//	reg show spec	display special  regs	
//	reg hide int	display general regs
//	reg hide float	display floating point regs
//	reg hide spec	display special  regs

		
	smallprintf( "\n reg - Display/Set register values"); 
	smallprintf( "\n Forms:"); 
	smallprintf( "\n reg             display ARM register values"); 

	smallprintf( "\n reg show all    show all registers"); 
	smallprintf( "\n reg hide all    hide all registers"); 
	smallprintf( "\n reg show int    show general registers"); 
	smallprintf( "\n reg hide int    hide general registers"); 
	smallprintf( "\n reg show float  show floating pont registers"); 
	smallprintf( "\n reg hide float  hide floating pont registers"); 
	smallprintf( "\n reg show spec   show special registers"); 
	smallprintf( "\n reg hide spec   hide special registers"); 

//		
//	reg show float	display floating point regs
//	reg show spec	display special  regs	
//	reg hide int	display general regs
//	reg hide float	display floating point regs
//	reg hide spec	display special  regs
	smallprintf( "\n reg NAME        display register value"); 
	smallprintf( "\n reg NAME VALUE  set register value"); 
	smallprintf( "\n **************"); 		
	smallprintf( "\n REGISTER NAMES:"); 
	smallprintf( "\n R0-R12 (general purpose registers)"); 
	smallprintf( "\n PC (program counter)"); 
	smallprintf( "\n SP (stack pointer)"); 
	smallprintf( "\n LR (link register)"); 
	smallprintf( "\n S0-R31 (floating point registers)"); 

}  

void	help_dm( void ) { 	
	smallprintf( "\n dm   - Display memory"); 
	smallprintf( "\n Forms:"); 
	smallprintf( "\n dm -b|h|w ADDRESS LENGTH display LENGTH byte/words from ADDRESS");
	smallprintf( "\n dm -b|h|w ADDRESS        display 256 bytes from ADDRESS");
	smallprintf( "\n dm  ADDRESS              display 256 bytes from ADDRESS");
}  

void	help_mm( void ) { 	
// mm -b|w|l ADDRESS VALUE 	
// mm -b|w|l ADDRESS    	
// mm ADDRESS 		
	smallprintf( "\n mm   - Modify memory");  		
	smallprintf( "\n Forms:");  
	smallprintf( "\n mm -b|h|w ADDRESS VALUE  set ADDRESS contents to VALUE"); 		
	smallprintf( "\n mm -b|h|w ADDRESS        enter modify memory mode"); 		
	smallprintf( "\n mm  ADDRESS              enter modify memory mode WORD"); 
	smallprintf( "\n Modify memory mode:"); 		
	smallprintf( "\n ADDRESS: VALUE<Enter>  modify"); 		
	smallprintf( "\n ADDRESS: +             next address"); 		
	smallprintf( "\n ADDRESS: -             previous address"); 		
	smallprintf( "\n ADDRESS: .             (dot) exit mm mode"); 
} 

void	help_dasm( void ) { 		
	smallprintf( "\n dasm - Disassemble");  		
	smallprintf( "\n Forms:");  		
	smallprintf( "\n dasm ADDRESS INS  disassemble INS instructions from ADDRESS");  		
	smallprintf( "\n dasm ADDRESS      disassemble one instruction from ADDRESS");  		
	smallprintf( "\n dasm              disassemble one instruction from current PC");  
}  

void	help_bp( void ) { 		
	smallprintf( "\n bp   - Breakpoints");  		
	smallprintf( "\n Forms:");  		
	smallprintf( "\n bp				show breakpoint table");  		
	smallprintf( "\n bp NUMBER set ADDRESS	set break at address");  		
	smallprintf( "\n bp NUMBER rem		remove break NUMBER");  		
	smallprintf( "\n bp NUMBER dis		disable break NUMBER");  		
	smallprintf( "\n bp NUMBER en		enable break NUMBER");  		
	smallprintf( "\n bp clear			clear breakpoint table"); 
	smallprintf("\n Maximum number of breakpoints is %d", MAXBP); 
}  


void	help_load( void ) { 		
	smallprintf( "\n load - Load");  		
	smallprintf( "\n Load S-records, no parameters");  
}



 void	help( int argc , unsigned char * cmd ) { 	
	smallprintf("\n *** CTH/ce ");
	smallprintf( DEBUGGER_NAME );
	smallprintf(" monitor/debugger HELP");

	if( argc == 1 ) 	{
		smallprintf( "\nType help 'command' for any of:");
		smallprintf( "\n tr   - Trace instruction");  		
		smallprintf( "\n go   - Run program");  		
		smallprintf( "\n reg  - Display/Modify registers");  		
		smallprintf( "\n dm   - Display memory");  		
		smallprintf( "\n mm   - Modify memory");  		
		smallprintf( "\n dasm - Disassemble");  		
		smallprintf( "\n bp   - Breakpoints");  		
		smallprintf( "\n load - Load S-records");

		return; 
	} 
	if( !strcmpi( "v", cmd  ) ){   		
		smallprintf("\n Compiled on: ");
		smallprintf( DATE_VERSION );
	} 	
	if( !strcmpi( "tr", cmd  ) ){   		
		help_tr(  ); 	
	} 	
	else if( !strcmpi( "go", cmd  ) ){   		
		help_go( ); 	
	} 	
	else if( !strcmpi( "reg", cmd  ) ){   		
		help_reg(  ); 	
	} 	
	else if( !strcmpi( "dm", cmd  ) ){   		
		help_dm( ); 	
	} 	
	else if( !strcmpi( "mm", cmd  ) ){   		
		help_mm( ); 	
	} 	
	else if( !strcmpi( "dasm", cmd  ) ){   		
		help_dasm(  ); 	
	} 	
	else if( !strcmpi( "bp", cmd  ) ){   		
		help_bp( ); 	
	}

	else if( !strcmpi( "l", cmd  ) ){   		
		help_load(); 	
	} 

}


 void main() {	
 /* 	main reads a CR terminated text string 	 
 *	converts into "argc argv" and invokes command  	 
 *	once main is called it never returns 	 */  
 char	*cp;
int test; 
 int	argc; 
 char	*argv[MAXARGS];   

	
	while(1){  
mainstart: 		
		smallprintf( PROMPTER );
		getstring( inbuf , SZOFINBUF , 0 ); 	

		argc=0; 	
		cp = skipblanc( inbuf );	
		// skip leading blancs 	  
		// fixup buffer and init 'argc' and 'argv' 	
		while( (noteol (*cp) ) && argc <= MAXARGS){ 		
			argv[argc]=cp; 	    	
			argc++; 		
			cp = skipnonblanc( cp ); 	    	
			if( ! noteol (*cp)){ 			
				*cp = '\0'; 			
				break;            
			} 	    	
			*cp++='\0'; 		
			cp = skipblanc( cp );	
			// skip blancs 	
		} 	 	
		if( argc == 0 ) 		
			goto mainstart;   	
		if( argc > MAXARGS ) 	
		{ 		
			smallprintf("\nillegal commandline"); 		
			goto mainstart; 	
		}  	
		if( inbuf[0] != '\n' )
		{	// ignore newlines... 
			if( (hottrace ==1) && (!strcmpi( ".", argv[0] )) )
			{ //	trace one instruction ... 			
				restore_vectorbase(1); /* copy all reserved vectors */
				trace ( 1 , &argv[0] ); 		
			}  		
			else if( (hottrace ==2) && (!strcmpi( ".", argv[0] )) )
			{ //	trace one instruction ... 			
				restore_vectorbase(0); /* copy debugger reserved vectors */
				trace ( 1 , &argv[0] ); 		
			}  		
			else if( !strcmpi( "tr", argv[0] ) )
			{ // trace 
				if( hottrace) hottrace = 1;
				restore_vectorbase(1); /* copy all reserved vectors */
				trace( argc , &argv[0] );  		
			} 		
			else if( !strcmpi( "tre", argv[0] ) )
			{ // trace 			
				if( hottrace) hottrace = 2;
				restore_vectorbase(0); /* copy debugger reserved vectors */
				trace( argc , &argv[0] );  		
			} 		
			else if( !strcmpi( "help", argv[0] ) )
			{
				help( argc, argv[1] ); 		
			}
			else if( !strcmpi( "dm", argv[0] ) )
			{  // display memory 			
				dm( argc , &argv[0] );  		
			} 		
			else if( !strcmpi( "mm", argv[0] ) )
			{   			// modify memory 			
				mm( argc , &argv[0] );  		
			}
			else if( !strcmpi( "reg", argv[0] ) )
			{  // display/modify registers ... 			
				reg( argc , &argv[0] );  		
			} 		
			else if( !strcmpi( "load", argv[0] ) )
			{ // load S-records			 			
				load(); 		
			}
			else if( !strcmpi( "dasm", argv[0] ) )
			{  // disassemble			 			
				dasm( argc , &argv[0] );  		
			}
			else if( !strcmpi( "bp", argv[0] ) )
			{  // breakpoint handling			 			
				breaks( argc , &argv[0] );  		
			} 		
			else if( !strcmpi( "go", argv[0] ) )
			{ // run ... 			
				restore_vectorbase(1); /* copy all reserved vectors */
				go( argc , &argv[0] );  		
			} 		
			else if( !strcmpi( "goe", argv[0] ) )
			{ // run ... 			
				restore_vectorbase(0); /* copy debugger reserved vectors */
				go( argc , &argv[0] );  		
			} 		
			else if( !strcmpi( "stat", argv[0] ) )
			{ // run ... 			
				prstat( );  		
			} 		
		
	
			else {
				smallprintf("\n\"%s\" not found", argv[0] ); 		
			} 	
		}   

	} 
}   
		
void	svc_call( unsigned int * call_args)
{	
	/* Dispatcher */
	unsigned int oscall = ((char *) call_args[6]) [-2];
	switch( oscall )
	{
	case	0xF0:	graphic_initalize(); break;
	case	0xF1:	graphic_clearScreen();	break;
	case	0xF2:	pixelset(call_args[0], call_args[1]); break;
	case	0xF3:	pixelclear(call_args[0], call_args[1]); break;
	}
}

__attribute__((naked))
void	SVC_Handler( void )	/* VARARGS ... */
{	/* Entry with exception stack */
	__asm volatile(" MOV	R0,SP\n");
	__asm volatile(" B		svc_call\n");
	__asm volatile(" .ALIGN	2\n");
}