#define FDMA_SRAM_TOP_ADDRESS 0xB9229800#define MAGIC_NUM 0x71097100#define DEAD_PATTERN 0xBAD0BADF#define ST40_CPG_REGS_BASE 0xFFC00000#defineST40_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");} |