#include "predef.h"
#include <stdio.h>
#include <ctype.h>
#include <basictypes.h>
// Use IRQ based peripherals for lower power, the processor uses less
// power when spinning in the idle task rather polling a peripheral. This
// can be seen by the 12+mA power increase at the initial getchar() when
// the polling UART driver is used.
#include <serialirq.h>
//#include <serialpoll.h>
#include <system.h>
#include <constants.h>
#include <ucos.h>
#include <SerialUpdate.h>
#include <smarttrap.h>
#include <sim.h>
#include <rtc.h>

extern "C" {
void UserMain(void * pd);
extern "C" void WRITE_CSR( WORD* );
}

const char * AppName="LowPowerMOD5213";


// Global defines
void PowerTest();

WORD wdebug_pstddata_off[4] = {0x2C80, 0x0002, 0x0000, 0x0000};
WORD wdebug_pstddata_on[4]  = {0x2C80, 0x0000, 0x0000, 0x0000};

#define PWR_MODE_STOP 0xC0
#define PWR_MODE_WAIT 0x80
#define PWR_MODE_DOZE 0x40
#define PWR_MODE_RUN  0x0
#define PWR_DISABLE_PLL    0x10
#define PWR_DISABLE_CLKOUT 0x08

// Shut down all peripherals except Flash module
// Getting interrupt to run from SRAM is a major pain plus it only saves 30uA
#define PPMRH_CONFIG 0x79B
#define PPMRL_CONFIG 0x1E6F0


void UserMain(void * pd) {
    SimpleUart(0,SystemBaud);
    assign_stdio(0);
    OSChangePrio(MAIN_PRIO);
    EnableSerialUpdate();
    #ifndef _DEBUG
    EnableSmartTraps();
    #endif

    iprintf("%s application started\n", AppName);

    WRITE_CSR(wdebug_pstddata_off); // Disable BDM pst/ddata signals

    while (1) {

    	iprintf("\r\nPress enter key to enter STOP mode\r\n");
		getchar();  // The polling UART driver will use about 15% more power here than the IRQ UART driver

		iprintf( ">> Processor Stoppping(SLOWING) \r\n" );
		OSTimeDly(1);  // Delay to finish printing before clock decrease

		/* Configure RTC to trigger IRQ in 5 seconds
		 * Function available in NNDK 23 and higher
		 * Requires jumper wire from RTC IRQ to MOD5213 IRQ1 */
		RTCTimerSeconds( 5 );

		PowerTest();     // Enter low power mode
		iprintf( "Processor Started \r\n" ); // If we print this we are running at full speed again
	}
}



/* This is the function will reduce the system clock, power down peripheral
   and then enter STOP mode.  After a delay or an IRQ wake, the function will
   bring the CPU back to normal settings before returning. */
void PowerTest()
{

   sim.scm.lpicr = 0x80 ;	   // Enable stop mode
   sim.cim.lpcr = ( PWR_MODE_STOP | PWR_DISABLE_PLL | PWR_DISABLE_CLKOUT );

   sim.clock.lpdr = 0xF;       // Clock low-power prescaler to slowest clock speed
   sim.clock.syncr = 0x7003;   // disconnect PLL
   sim.clock.syncr = 0x0702;   // shut down PLL and decrease system speed with prescaller
   sim.scm.ppmrh |= PPMRH_CONFIG;     // disable peripheral clocks
   sim.scm.ppmrl |= PPMRL_CONFIG;
   asm(" stop #0x2000 ");      // ENTER STOP MODE
   //for(volatile int x = 0; x<50; x++) asm("nop");  // Delay for reading current, not needed for STOP mode

   sim.clock.lpdr = 0x00;         // Clock low-power prescaler to default clock speed
   sim.clock.syncr = 0x7107;      // Connect PLL and increase system clock speed RFD+1 to avoid over-clock
   while( ( sim.clock.synsr & 0x8 ) != 0x8);  // Wait for PLL to lock at set multiplier
   sim.clock.syncr = 0x7007;      // Now it is safe to increase speed to full 66MHz

   // Enable peripheral power registers to enable peripheral clocks
   sim.scm.ppmrl &= ~PPMRL_CONFIG;
   sim.scm.ppmrh &= ~PPMRH_CONFIG;
   return;
}

// Function to write to CSR register
   asm(".global WRITE_CSR");
   asm("WRITE_CSR:");
   asm(" move.l %sp@(4), %a0 ");
   asm(" wdebug %a0@ ");
   asm(" rts ");
   asm("WRITE_CSREND:");