#define FDMA_SRAM_TOP_ADDRESS 0xB9229800
#define MAGIC_NUM 0x71097100
#define DEAD_PATTERN 0xBAD0BADF
#define ST40_CPG_REGS_BASE 0xFFC00000#define
ST40_CPG_WTCNT (ST40_CPG_REGS_BASE + 0x08)
#define ST40_CPG_WTCSR (ST40_CPG_REGS_BASE + 0x0C)
#define ST40_CPG_WTCSR2 (ST40_CPG_REGS_BASE + 0x1C)
void Jump(unsigned int address)
{
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*1, MAGIC_NUM);
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*2, address);
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*3, ~address);
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*4, ~MAGIC_NUM); 
STSYS_WriteRegDev16LE(ST40_CPG_WTCNT, 0x5AF0); /*Watchdog counter*/
STSYS_WriteRegDev16LE(ST40_CPG_WTCSR, 0xA547); /*Watchdog control*/
STSYS_WriteRegDev16LE(ST40_CPG_WTCSR2, 0xAA00); /*Watchdog control2*/ 
STSYS_WriteRegDev16LE(ST40_CPG_WTCSR, 0xA5C7); /*Start watchdog counter*/ for(;;);
}
void SelectBoot(void)
{
unsigned int MagicNum = 0;
unsigned int NotMagicNum = 0;
unsigned int JumpAddress = 0;
unsigned int NotJumpAddress = 0; void (*entry)(void);
/* entry=0xa0100000;
entry();*/ //if((STSYS_ReadRegDev16LE(ST40_CPG_WTCSR2) & 0xFF) != 0x0)
{
MagicNum = STSYS_ReadRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*1);
JumpAddress = STSYS_ReadRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*2);
NotJumpAddress = STSYS_ReadRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*3);
NotMagicNum = STSYS_ReadRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*4); 
if((MagicNum == MAGIC_NUM)
&& (MagicNum == (~NotMagicNum))
&& (JumpAddress == (~NotJumpAddress)))
{
entry = (void (*)(void))JumpAddress;
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*1, DEAD_PATTERN);
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*2, DEAD_PATTERN);
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*3, DEAD_PATTERN);
STSYS_WriteRegDev32LE(FDMA_SRAM_TOP_ADDRESS - 4*4, DEAD_PATTERN); 
#if 0/*Setup For watchdog in case jump failed.*/
STSYS_WriteRegDev16LE(ST40_CPG_WTCNT, 0x5AF0);
STSYS_WriteRegDev16LE(ST40_CPG_WTCSR, 0xA543);
STSYS_WriteRegDev16LE(ST40_CPG_WTCSR2, 0xAA01); 
STSYS_WriteRegDev16LE(ST40_CPG_WTCSR, 0xA5C7);
#endif entry(); while(1);
}
}
}void system_reboot(void)
{
ulong sr;
asm ("stc sr, %0":"=r" (sr));
sr |= (1 << 28); /* set block bit */
asm ("ldc %0, sr": :"r" (sr));
asm volatile ("trapa #0");
}