#define uint32 unsigned long

#define OFF	0x00,0x28
#define	BLUE	0x08,0x20
#define	GREEN	0x20,0x08

//Setup MFP and direction configs for the LED GPIOs
void setupLEDs(void){
	uint32 *dirGPIO = (uint32 *)0x40e0000c;
	uint32 *mfprGPIO3 = (uint32 *)0x40e1027c;
	uint32 *mfprGPIO3_2 = (uint32 *)0x40e102e0;
	uint32 *mfprGPIO5 = (uint32 *)0x40e10284;
	uint32 *mfprGPIO5_2 = (uint32 *)0x40e102e8;

	*dirGPIO |= 0x28;			//GPIOs as outputs

	*mfprGPIO3 = 0x0844;			//Make sure GPIOs 3 and 5 are not set to the pins.
	*mfprGPIO5 = 0x0844;
	*mfprGPIO3_2 = 0x8C40;			//Then use GPIOs 3_2 / 5_2 for the blue/green LED.
	*mfprGPIO5_2 = 0x8C40;	
}

//set the LED states by turning on/off GPIOs in the first bank (use the defines to pass both params)
void setLEDs(uint32 on, uint32 off){
	uint32 *setGPIO = (uint32 *)0x40e00018;
	uint32 *clearGPIO = (uint32 *)0x40e00024;

	*clearGPIO = off;
	*setGPIO = on;	
}

//Displays a single error code (possibly repeatedly)
//Single blue flash followed by 'code' number of green flashes
#define DELAY 500000
void error(uint32 code, int freeze){
	int i,j;

	do{
		setLEDs(BLUE);
		for(i=0;i<3*DELAY;i++) __asm("nop");

		setLEDs(OFF);
		for(i=0;i<2*DELAY;i++) __asm("nop");

		for(j=0;j<code;j++){
			setLEDs(OFF);
			for(i=0;i<DELAY;i++) __asm("nop");

			setLEDs(GREEN);
			for(i=0;i<DELAY;i++) __asm("nop");
		}
		setLEDs(OFF);
		for(i=0;i<5*DELAY;i++) __asm("nop");
	}while(freeze);
}

//Outputs (slowly) a 32-bit uint
//Each nibble is a blue flash and then v green flashes (0-15)
void readUInt(uint32 val){
	int i,j,k,v;
	for(i=28;i>=0;i-=4){
		v = (val >> i) & 0x0f;

		setLEDs(BLUE);
		for(k=0;k<3*DELAY;k++) __asm("nop");
		setLEDs(OFF);
		for(k=0;k<2*DELAY;k++) __asm("nop");

		for(j=0;j<v;j++){
			setLEDs(OFF);
			for(k=0;k<DELAY;k++) __asm("nop");

			setLEDs(GREEN);
			for(k=0;k<DELAY;k++) __asm("nop");
		}

		setLEDs(OFF);
		for(k=0;k<5*DELAY;k++) __asm("nop");
	}
}