[proxmark3-svn] / bootrom / bootrom.c

#include <proxmark3.h>\r
\r
struct common_area common_area __attribute__((section(".commonarea")));\r
unsigned int start_addr, end_addr, bootrom_unlocked;\r
extern char _bootrom_start, _bootrom_end, _flash_start, _flash_end;\r
\r
static void ConfigClocks(void)\r
{\r
    // we are using a 16 MHz crystal as the basis for everything\r
    // slow clock runs at 32Khz typical regardless of crystal\r
\r
    // enable system clock and USB clock\r
    AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_PCK | AT91C_PMC_UDP;\r
\r
	// enable the clock to the following peripherals\r
    AT91C_BASE_PMC->PMC_PCER =\r
		(1<<AT91C_ID_PIOA)	|\r
		(1<<AT91C_ID_ADC)	|\r
		(1<<AT91C_ID_SPI)	|\r
		(1<<AT91C_ID_SSC)	|\r
		(1<<AT91C_ID_PWMC)	|\r
		(1<<AT91C_ID_UDP);\r
\r
	// worst case scenario, with 16Mhz xtal startup delay is 14.5ms\r
	// with a slow clock running at it worst case (max) frequency of 42khz\r
	// max startup delay = (14.5ms*42k)/8 = 76 = 0x4C round up to 0x50\r
\r
	// enable main oscillator and set startup delay\r
    AT91C_BASE_PMC->PMC_MOR =\r
    	PMC_MAIN_OSC_ENABLE |\r
        PMC_MAIN_OSC_STARTUP_DELAY(0x50);\r
\r
	// wait for main oscillator to stabilize\r
	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_STABILIZED) )\r
		;\r
\r
    // minimum PLL clock frequency is 80 MHz in range 00 (96 here so okay)\r
    // frequency is crystal * multiplier / divisor = 16Mhz * 12 / 2 = 96Mhz\r
    AT91C_BASE_PMC->PMC_PLLR =\r
    	PMC_PLL_DIVISOR(2) |\r
		PMC_PLL_COUNT_BEFORE_LOCK(0x50) |\r
		PMC_PLL_FREQUENCY_RANGE(0) |\r
		PMC_PLL_MULTIPLIER(12) |\r
		PMC_PLL_USB_DIVISOR(1);\r
\r
	// wait for PLL to lock\r
	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_PLL_LOCK) )\r
		;\r
\r
	// we want a master clock (MCK) to be PLL clock / 2 = 96Mhz / 2 = 48Mhz\r
	// as per datasheet, this register must be programmed in two operations\r
	// when changing to PLL, program the prescaler first then the source\r
    AT91C_BASE_PMC->PMC_MCKR = PMC_CLK_PRESCALE_DIV_2;\r
\r
	// wait for main clock ready signal\r
	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_MCK_READY) )\r
		;\r
\r
	// set the source to PLL\r
    AT91C_BASE_PMC->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK | PMC_CLK_PRESCALE_DIV_2;\r
\r
	// wait for main clock ready signal\r
	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_MCK_READY) )\r
		;\r
}\r
\r
static void Fatal(void)\r
{\r
    for(;;);\r
}\r
\r
void UsbPacketReceived(BYTE *packet, int len)\r
{\r
    int i, dont_ack=0;\r
    UsbCommand *c = (UsbCommand *)packet;\r
    volatile DWORD *p;\r
\r
    if(len != sizeof(*c)) {\r
        Fatal();\r
    }\r
\r
    switch(c->cmd) {\r
        case CMD_DEVICE_INFO:\r
            dont_ack = 1;\r
            c->cmd = CMD_DEVICE_INFO;\r
            c->ext1 = DEVICE_INFO_FLAG_BOOTROM_PRESENT | DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM |\r
                DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH;\r
            if(common_area.flags.osimage_present) c->ext1 |= DEVICE_INFO_FLAG_OSIMAGE_PRESENT;\r
            UsbSendPacket(packet, len);\r
            break;\r
\r
        case CMD_SETUP_WRITE:\r
            /* The temporary write buffer of the embedded flash controller is mapped to the\r
             * whole memory region, only the last 8 bits are decoded.\r
             */\r
            p = (volatile DWORD *)&_flash_start;\r
            for(i = 0; i < 12; i++) {\r
                p[i+c->ext1] = c->d.asDwords[i];\r
            }\r
            break;\r
\r
        case CMD_FINISH_WRITE:\r
            p = (volatile DWORD *)&_flash_start;\r
            for(i = 0; i < 4; i++) {\r
                p[i+60] = c->d.asDwords[i];\r
            }\r
\r
            /* Check that the address that we are supposed to write to is within our allowed region */\r
            if( ((c->ext1+AT91C_IFLASH_PAGE_SIZE-1) >= end_addr) || (c->ext1 < start_addr) ) {\r
                /* Disallow write */\r
                dont_ack = 1;\r
                c->cmd = CMD_NACK;\r
                UsbSendPacket(packet, len);\r
            } else {\r
                /* Translate address to flash page and do flash, update here for the 512k part */\r
                AT91C_BASE_EFC0->EFC_FCR = MC_FLASH_COMMAND_KEY |\r
                    MC_FLASH_COMMAND_PAGEN((c->ext1-(int)&_flash_start)/AT91C_IFLASH_PAGE_SIZE) |\r
                    AT91C_MC_FCMD_START_PROG;\r
            }\r
            while(!(AT91C_BASE_EFC0->EFC_FSR & MC_FLASH_STATUS_READY))\r
                ;\r
            break;\r
\r
        case CMD_HARDWARE_RESET:\r
            USB_D_PLUS_PULLUP_OFF();\r
            AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST;\r
            break;\r
\r
        case CMD_START_FLASH:\r
            if(c->ext3 == START_FLASH_MAGIC) bootrom_unlocked = 1;\r
            else bootrom_unlocked = 0;\r
            {\r
                int prot_start = (int)&_bootrom_start;\r
                int prot_end = (int)&_bootrom_end;\r
                int allow_start = (int)&_flash_start;\r
                int allow_end = (int)&_flash_end;\r
                int cmd_start = c->ext1;\r
                int cmd_end = c->ext2;\r
\r
                /* Only allow command if the bootrom is unlocked, or the parameters are outside of the protected\r
                 * bootrom area. In any case they must be within the flash area.\r
                 */\r
                if( (bootrom_unlocked || ((cmd_start >= prot_end) || (cmd_end < prot_start)))\r
                    && (cmd_start >= allow_start) && (cmd_end <= allow_end) ) {\r
                    start_addr = cmd_start;\r
                    end_addr = cmd_end;\r
                } else {\r
                    start_addr = end_addr = 0;\r
                    dont_ack = 1;\r
                    c->cmd = CMD_NACK;\r
                    UsbSendPacket(packet, len);\r
                }\r
            }\r
            break;\r
\r
        default:\r
            Fatal();\r
            break;\r
    }\r
\r
    if(!dont_ack) {\r
        c->cmd = CMD_ACK;\r
        UsbSendPacket(packet, len);\r
    }\r
}\r
\r
static void flash_mode(int externally_entered)\r
{\r
	start_addr = 0;\r
	end_addr = 0;\r
	bootrom_unlocked = 0;\r
\r
	UsbStart();\r
	for(;;) {\r
		WDT_HIT();\r
\r
		UsbPoll(TRUE);\r
\r
		if(!externally_entered && !BUTTON_PRESS()) {\r
			/* Perform a reset to leave flash mode */\r
			USB_D_PLUS_PULLUP_OFF();\r
			LED_B_ON();\r
			AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY | AT91C_RSTC_PROCRST;\r
			for(;;);\r
		}\r
		if(externally_entered && BUTTON_PRESS()) {\r
			/* Let the user's button press override the automatic leave */\r
			externally_entered = 0;\r
		}\r
	}\r
}\r
\r
extern char _osimage_entry;\r
void BootROM(void)\r
{\r
    //------------\r
    // First set up all the I/O pins; GPIOs configured directly, other ones\r
    // just need to be assigned to the appropriate peripheral.\r
\r
    // Kill all the pullups, especially the one on USB D+; leave them for\r
    // the unused pins, though.\r
    AT91C_BASE_PIOA->PIO_PPUDR =\r
    	GPIO_USB_PU			|\r
		GPIO_LED_A			|\r
		GPIO_LED_B			|\r
		GPIO_LED_C			|\r
		GPIO_LED_D			|\r
		GPIO_FPGA_DIN		|\r
		GPIO_FPGA_DOUT		|\r
		GPIO_FPGA_CCLK		|\r
		GPIO_FPGA_NINIT		|\r
		GPIO_FPGA_NPROGRAM	|\r
		GPIO_FPGA_DONE		|\r
		GPIO_MUXSEL_HIPKD	|\r
		GPIO_MUXSEL_HIRAW	|\r
		GPIO_MUXSEL_LOPKD	|\r
		GPIO_MUXSEL_LORAW	|\r
		GPIO_RELAY			|\r
		GPIO_NVDD_ON;\r
		// (and add GPIO_FPGA_ON)\r
	// These pins are outputs\r
    AT91C_BASE_PIOA->PIO_OER =\r
    	GPIO_LED_A			|\r
		GPIO_LED_B			|\r
		GPIO_LED_C			|\r
		GPIO_LED_D			|\r
		GPIO_RELAY			|\r
		GPIO_NVDD_ON;\r
	// PIO controls the following pins\r
    AT91C_BASE_PIOA->PIO_PER =\r
    	GPIO_USB_PU			|\r
		GPIO_LED_A			|\r
		GPIO_LED_B			|\r
		GPIO_LED_C			|\r
		GPIO_LED_D;\r
\r
    USB_D_PLUS_PULLUP_OFF();\r
    LED_D_OFF();\r
    LED_C_ON();\r
    LED_B_OFF();\r
    LED_A_OFF();\r
\r
    // if 512K FLASH part - TODO make some defines :)\r
    if ((AT91C_BASE_DBGU->DBGU_CIDR | 0xf00) == 0xa00) {\r
	    AT91C_BASE_EFC0->EFC_FMR =\r
	    	MC_FLASH_MODE_FLASH_WAIT_STATES(1) |\r
			MC_FLASH_MODE_MASTER_CLK_IN_MHZ(0x48);\r
	    AT91C_BASE_EFC1->EFC_FMR =\r
	    	MC_FLASH_MODE_FLASH_WAIT_STATES(1) |\r
			MC_FLASH_MODE_MASTER_CLK_IN_MHZ(0x48);\r
    } else {\r
	    AT91C_BASE_EFC0->EFC_FMR =\r
	    	MC_FLASH_MODE_FLASH_WAIT_STATES(0) |\r
			MC_FLASH_MODE_MASTER_CLK_IN_MHZ(48);\r
    }\r
\r
    // Initialize all system clocks\r
    ConfigClocks();\r
\r
    LED_A_ON();\r
\r
    int common_area_present = 0;\r
    switch(AT91C_BASE_RSTC->RSTC_RSR & AT91C_RSTC_RSTTYP) {\r
    case AT91C_RSTC_RSTTYP_WATCHDOG:\r
    case AT91C_RSTC_RSTTYP_SOFTWARE:\r
    case AT91C_RSTC_RSTTYP_USER:\r
	    /* In these cases the common_area in RAM should be ok, retain it if it's there */\r
	    if(common_area.magic == COMMON_AREA_MAGIC && common_area.version == 1) {\r
		    common_area_present = 1;\r
	    }\r
	    break;\r
    default: /* Otherwise, initialize it from scratch */\r
	    break;\r
    }\r
\r
    if(!common_area_present){\r
	    /* Common area not ok, initialize it */\r
	    int i; for(i=0; i<sizeof(common_area); i++) { /* Makeshift memset, no need to drag util.c into this */\r
		    ((char*)&common_area)[i] = 0;\r
	    }\r
	    common_area.magic = COMMON_AREA_MAGIC;\r
	    common_area.version = 1;\r
	    common_area.flags.bootrom_present = 1;\r
    }\r
\r
    common_area.flags.bootrom_present = 1;\r
    if(common_area.command == COMMON_AREA_COMMAND_ENTER_FLASH_MODE) {\r
	    common_area.command = COMMON_AREA_COMMAND_NONE;\r
	    flash_mode(1);\r
    } else if(BUTTON_PRESS()) {\r
	    flash_mode(0);\r
    } else {\r
	    // jump to Flash address of the osimage entry point (LSBit set for thumb mode)\r
	    asm("bx %0\n" : : "r" ( ((int)&_osimage_entry) | 0x1 ) );\r
    }\r
}\r
Commit	Line	Data
6658905f	1	#include <proxmark3.h>\r
6658905f	2	\r
8fcbf652	3	struct common_area common_area __attribute__((section(".commonarea")));\r
6949aca9	4	unsigned int start_addr, end_addr, bootrom_unlocked;\r
8fcbf652	5	extern char _bootrom_start, _bootrom_end, _flash_start, _flash_end;\r
8fcbf652	6	\r
6658905f	7	static void ConfigClocks(void)\r
	8	{\r
	9	// we are using a 16 MHz crystal as the basis for everything\r
	10	// slow clock runs at 32Khz typical regardless of crystal\r
	11	\r
	12	// enable system clock and USB clock\r
6949aca9	13	AT91C_BASE_PMC->PMC_SCER = AT91C_PMC_PCK \| AT91C_PMC_UDP;\r
6658905f	14	\r
6658905f	15	// enable the clock to the following peripherals\r
6949aca9	16	AT91C_BASE_PMC->PMC_PCER =\r
	17	(1<<AT91C_ID_PIOA) \|\r
	18	(1<<AT91C_ID_ADC) \|\r
	19	(1<<AT91C_ID_SPI) \|\r
	20	(1<<AT91C_ID_SSC) \|\r
	21	(1<<AT91C_ID_PWMC) \|\r
	22	(1<<AT91C_ID_UDP);\r
6658905f	23	\r
	24	// worst case scenario, with 16Mhz xtal startup delay is 14.5ms\r
	25	// with a slow clock running at it worst case (max) frequency of 42khz\r
	26	// max startup delay = (14.5ms*42k)/8 = 76 = 0x4C round up to 0x50\r
	27	\r
	28	// enable main oscillator and set startup delay\r
6949aca9	29	AT91C_BASE_PMC->PMC_MOR =\r
	30	PMC_MAIN_OSC_ENABLE \|\r
	31	PMC_MAIN_OSC_STARTUP_DELAY(0x50);\r
6658905f	32	\r
6658905f	33	// wait for main oscillator to stabilize\r
6949aca9	34	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_STABILIZED) )\r
6658905f	35	;\r
	36	\r
	37	// minimum PLL clock frequency is 80 MHz in range 00 (96 here so okay)\r
	38	// frequency is crystal * multiplier / divisor = 16Mhz * 12 / 2 = 96Mhz\r
6949aca9	39	AT91C_BASE_PMC->PMC_PLLR =\r
	40	PMC_PLL_DIVISOR(2) \|\r
	41	PMC_PLL_COUNT_BEFORE_LOCK(0x50) \|\r
	42	PMC_PLL_FREQUENCY_RANGE(0) \|\r
	43	PMC_PLL_MULTIPLIER(12) \|\r
	44	PMC_PLL_USB_DIVISOR(1);\r
6658905f	45	\r
6658905f	46	// wait for PLL to lock\r
6949aca9	47	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_PLL_LOCK) )\r
6658905f	48	;\r
	49	\r
	50	// we want a master clock (MCK) to be PLL clock / 2 = 96Mhz / 2 = 48Mhz\r
	51	// as per datasheet, this register must be programmed in two operations\r
	52	// when changing to PLL, program the prescaler first then the source\r
6949aca9	53	AT91C_BASE_PMC->PMC_MCKR = PMC_CLK_PRESCALE_DIV_2;\r
6658905f	54	\r
6658905f	55	// wait for main clock ready signal\r
6949aca9	56	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_MCK_READY) )\r
6658905f	57	;\r
	58	\r
	59	// set the source to PLL\r
6949aca9	60	AT91C_BASE_PMC->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK \| PMC_CLK_PRESCALE_DIV_2;\r
6658905f	61	\r
6658905f	62	// wait for main clock ready signal\r
6949aca9	63	while ( !(AT91C_BASE_PMC->PMC_SR & PMC_MAIN_OSC_MCK_READY) )\r
6658905f	64	;\r
	65	}\r
	66	\r
	67	static void Fatal(void)\r
	68	{\r
	69	for(;;);\r
	70	}\r
	71	\r
	72	void UsbPacketReceived(BYTE *packet, int len)\r
	73	{\r
8fcbf652	74	int i, dont_ack=0;\r
6658905f	75	UsbCommand c = (UsbCommand )packet;\r
	76	volatile DWORD *p;\r
	77	\r
	78	if(len != sizeof(*c)) {\r
	79	Fatal();\r
	80	}\r
	81	\r
	82	switch(c->cmd) {\r
	83	case CMD_DEVICE_INFO:\r
8fcbf652	84	dont_ack = 1;\r
8fcbf652	85	c->cmd = CMD_DEVICE_INFO;\r
6949aca9	86	c->ext1 = DEVICE_INFO_FLAG_BOOTROM_PRESENT \| DEVICE_INFO_FLAG_CURRENT_MODE_BOOTROM \|\r
8fcbf652	87	DEVICE_INFO_FLAG_UNDERSTANDS_START_FLASH;\r
	88	if(common_area.flags.osimage_present) c->ext1 \|= DEVICE_INFO_FLAG_OSIMAGE_PRESENT;\r
	89	UsbSendPacket(packet, len);\r
6658905f	90	break;\r
	91	\r
	92	case CMD_SETUP_WRITE:\r
8fcbf652	93	/* The temporary write buffer of the embedded flash controller is mapped to the\r
6949aca9	94	* whole memory region, only the last 8 bits are decoded.\r
8fcbf652	95	*/\r
8fcbf652	96	p = (volatile DWORD *)&_flash_start;\r
6658905f	97	for(i = 0; i < 12; i++) {\r
	98	p[i+c->ext1] = c->d.asDwords[i];\r
	99	}\r
	100	break;\r
	101	\r
	102	case CMD_FINISH_WRITE:\r
8fcbf652	103	p = (volatile DWORD *)&_flash_start;\r
6658905f	104	for(i = 0; i < 4; i++) {\r
	105	p[i+60] = c->d.asDwords[i];\r
	106	}\r
	107	\r
8fcbf652	108	/* Check that the address that we are supposed to write to is within our allowed region */\r
6949aca9	109	if( ((c->ext1+AT91C_IFLASH_PAGE_SIZE-1) >= end_addr) \|\| (c->ext1 < start_addr) ) {\r
8fcbf652	110	/* Disallow write */\r
	111	dont_ack = 1;\r
	112	c->cmd = CMD_NACK;\r
	113	UsbSendPacket(packet, len);\r
	114	} else {\r
	115	/* Translate address to flash page and do flash, update here for the 512k part */\r
6949aca9	116	AT91C_BASE_EFC0->EFC_FCR = MC_FLASH_COMMAND_KEY \|\r
	117	MC_FLASH_COMMAND_PAGEN((c->ext1-(int)&_flash_start)/AT91C_IFLASH_PAGE_SIZE) \|\r
	118	AT91C_MC_FCMD_START_PROG;\r
8fcbf652	119	}\r
6949aca9	120	while(!(AT91C_BASE_EFC0->EFC_FSR & MC_FLASH_STATUS_READY))\r
6658905f	121	;\r
	122	break;\r
	123	\r
	124	case CMD_HARDWARE_RESET:\r
8fcbf652	125	USB_D_PLUS_PULLUP_OFF();\r
6949aca9	126	AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY \| AT91C_RSTC_PROCRST;\r
6658905f	127	break;\r
6949aca9	128	\r
8fcbf652	129	case CMD_START_FLASH:\r
	130	if(c->ext3 == START_FLASH_MAGIC) bootrom_unlocked = 1;\r
	131	else bootrom_unlocked = 0;\r
	132	{\r
	133	int prot_start = (int)&_bootrom_start;\r
	134	int prot_end = (int)&_bootrom_end;\r
	135	int allow_start = (int)&_flash_start;\r
	136	int allow_end = (int)&_flash_end;\r
	137	int cmd_start = c->ext1;\r
	138	int cmd_end = c->ext2;\r
6949aca9	139	\r
8fcbf652	140	/* Only allow command if the bootrom is unlocked, or the parameters are outside of the protected\r
6949aca9	141	* bootrom area. In any case they must be within the flash area.\r
8fcbf652	142	*/\r
	143	if( (bootrom_unlocked \|\| ((cmd_start >= prot_end) \|\| (cmd_end < prot_start)))\r
	144	&& (cmd_start >= allow_start) && (cmd_end <= allow_end) ) {\r
	145	start_addr = cmd_start;\r
	146	end_addr = cmd_end;\r
	147	} else {\r
	148	start_addr = end_addr = 0;\r
	149	dont_ack = 1;\r
	150	c->cmd = CMD_NACK;\r
	151	UsbSendPacket(packet, len);\r
	152	}\r
	153	}\r
	154	break;\r
6949aca9	155	\r
6658905f	156	default:\r
	157	Fatal();\r
	158	break;\r
	159	}\r
	160	\r
8fcbf652	161	if(!dont_ack) {\r
	162	c->cmd = CMD_ACK;\r
	163	UsbSendPacket(packet, len);\r
	164	}\r
	165	}\r
	166	\r
	167	static void flash_mode(int externally_entered)\r
	168	{\r
	169	start_addr = 0;\r
	170	end_addr = 0;\r
	171	bootrom_unlocked = 0;\r
6949aca9	172	\r
8fcbf652	173	UsbStart();\r
	174	for(;;) {\r
	175	WDT_HIT();\r
6949aca9	176	\r
8fcbf652	177	UsbPoll(TRUE);\r
6949aca9	178	\r
8fcbf652	179	if(!externally_entered && !BUTTON_PRESS()) {\r
	180	/* Perform a reset to leave flash mode */\r
	181	USB_D_PLUS_PULLUP_OFF();\r
	182	LED_B_ON();\r
6949aca9	183	AT91C_BASE_RSTC->RSTC_RCR = RST_CONTROL_KEY \| AT91C_RSTC_PROCRST;\r
8fcbf652	184	for(;;);\r
	185	}\r
	186	if(externally_entered && BUTTON_PRESS()) {\r
	187	/* Let the user's button press override the automatic leave */\r
	188	externally_entered = 0;\r
	189	}\r
	190	}\r
6658905f	191	}\r
6658905f	192	\r
e3ae0257	193	extern char _osimage_entry;\r
6658905f	194	void BootROM(void)\r
	195	{\r
	196	//------------\r
	197	// First set up all the I/O pins; GPIOs configured directly, other ones\r
	198	// just need to be assigned to the appropriate peripheral.\r
	199	\r
	200	// Kill all the pullups, especially the one on USB D+; leave them for\r
	201	// the unused pins, though.\r
6949aca9	202	AT91C_BASE_PIOA->PIO_PPUDR =\r
	203	GPIO_USB_PU \|\r
	204	GPIO_LED_A \|\r
	205	GPIO_LED_B \|\r
	206	GPIO_LED_C \|\r
	207	GPIO_LED_D \|\r
	208	GPIO_FPGA_DIN \|\r
	209	GPIO_FPGA_DOUT \|\r
	210	GPIO_FPGA_CCLK \|\r
	211	GPIO_FPGA_NINIT \|\r
	212	GPIO_FPGA_NPROGRAM \|\r
	213	GPIO_FPGA_DONE \|\r
	214	GPIO_MUXSEL_HIPKD \|\r
	215	GPIO_MUXSEL_HIRAW \|\r
	216	GPIO_MUXSEL_LOPKD \|\r
	217	GPIO_MUXSEL_LORAW \|\r
	218	GPIO_RELAY \|\r
	219	GPIO_NVDD_ON;\r
	220	// (and add GPIO_FPGA_ON)\r
6658905f	221	// These pins are outputs\r
6949aca9	222	AT91C_BASE_PIOA->PIO_OER =\r
	223	GPIO_LED_A \|\r
	224	GPIO_LED_B \|\r
	225	GPIO_LED_C \|\r
	226	GPIO_LED_D \|\r
	227	GPIO_RELAY \|\r
	228	GPIO_NVDD_ON;\r
6658905f	229	// PIO controls the following pins\r
6949aca9	230	AT91C_BASE_PIOA->PIO_PER =\r
	231	GPIO_USB_PU \|\r
	232	GPIO_LED_A \|\r
	233	GPIO_LED_B \|\r
	234	GPIO_LED_C \|\r
	235	GPIO_LED_D;\r
6658905f	236	\r
	237	USB_D_PLUS_PULLUP_OFF();\r
	238	LED_D_OFF();\r
	239	LED_C_ON();\r
	240	LED_B_OFF();\r
	241	LED_A_OFF();\r
6949aca9	242	\r
8fcbf652	243	// if 512K FLASH part - TODO make some defines :)\r
6949aca9	244	if ((AT91C_BASE_DBGU->DBGU_CIDR \| 0xf00) == 0xa00) {\r
	245	AT91C_BASE_EFC0->EFC_FMR =\r
	246	MC_FLASH_MODE_FLASH_WAIT_STATES(1) \|\r
	247	MC_FLASH_MODE_MASTER_CLK_IN_MHZ(0x48);\r
	248	AT91C_BASE_EFC1->EFC_FMR =\r
	249	MC_FLASH_MODE_FLASH_WAIT_STATES(1) \|\r
	250	MC_FLASH_MODE_MASTER_CLK_IN_MHZ(0x48);\r
8fcbf652	251	} else {\r
6949aca9	252	AT91C_BASE_EFC0->EFC_FMR =\r
	253	MC_FLASH_MODE_FLASH_WAIT_STATES(0) \|\r
	254	MC_FLASH_MODE_MASTER_CLK_IN_MHZ(48);\r
8fcbf652	255	}\r
6949aca9	256	\r
8fcbf652	257	// Initialize all system clocks\r
6658905f	258	ConfigClocks();\r
6949aca9	259	\r
6658905f	260	LED_A_ON();\r
6949aca9	261	\r
8fcbf652	262	int common_area_present = 0;\r
6949aca9	263	switch(AT91C_BASE_RSTC->RSTC_RSR & AT91C_RSTC_RSTTYP) {\r
	264	case AT91C_RSTC_RSTTYP_WATCHDOG:\r
	265	case AT91C_RSTC_RSTTYP_SOFTWARE:\r
	266	case AT91C_RSTC_RSTTYP_USER:\r
8fcbf652	267	/* In these cases the common_area in RAM should be ok, retain it if it's there */\r
	268	if(common_area.magic == COMMON_AREA_MAGIC && common_area.version == 1) {\r
	269	common_area_present = 1;\r
	270	}\r
	271	break;\r
	272	default: /* Otherwise, initialize it from scratch */\r
	273	break;\r
	274	}\r
6949aca9	275	\r
8fcbf652	276	if(!common_area_present){\r
	277	/* Common area not ok, initialize it */\r
	278	int i; for(i=0; i<sizeof(common_area); i++) { /* Makeshift memset, no need to drag util.c into this */\r
	279	((char*)&common_area)[i] = 0;\r
	280	}\r
	281	common_area.magic = COMMON_AREA_MAGIC;\r
	282	common_area.version = 1;\r
	283	common_area.flags.bootrom_present = 1;\r
	284	}\r
6949aca9	285	\r
8fcbf652	286	common_area.flags.bootrom_present = 1;\r
	287	if(common_area.command == COMMON_AREA_COMMAND_ENTER_FLASH_MODE) {\r
	288	common_area.command = COMMON_AREA_COMMAND_NONE;\r
	289	flash_mode(1);\r
	290	} else if(BUTTON_PRESS()) {\r
	291	flash_mode(0);\r
	292	} else {\r
	293	// jump to Flash address of the osimage entry point (LSBit set for thumb mode)\r
	294	asm("bx %0\n" : : "r" ( ((int)&_osimage_entry) \| 0x1 ) );\r
6658905f	295	}\r
6658905f	296	}\r