#include "common.h"
#include "ndsloader_bin.h"

static void resetArm9();
static void bootstrapArm7();
static void bootstrapArm7DSiSync();
static int sendipcmsg(u32 channel, u32 data);

static inline void debug_color(int r, int g, int b)
{
	u16 clr = RGB15(r,g,b);
	*(vu32*)0x05000000 = clr;
	*(vu32*)0x05000400 = clr;
}

int main(void)
{
	debug_color(31,15,0); // orange

	resetArm9();
	bootstrapArm7();
	__builtin_unreachable();
}

void resetArm9()
{
	int i, reg;

	for(i=0; i<4; i++)
	{
		DMA_CR(i) = 0;//Reset DMA.
		DMA_SRC(i) = 0;
		DMA_DEST(i) = 0;
		TIMER_CR(i) = 0;//Reset timers.
		TIMER_DATA(i) = 0;
		for(reg=0; reg<0x1c; reg+=4)*((u32*)(0x04004104 + ((i*0x1c)+reg))) = 0;//Reset NDMA.
	}

	VRAM_CR = 0;
	VRAM_EFG_CR = 0;
	VRAM_H_CR = 0;
	VRAM_I_CR = 0;

	REG_POWERCNT = 0x820f;
	WRAM_CR = 0x03;
	REG_EXMEMCNT = 0xE880;
}

int sendipcmsg(u32 chan, u32 data)
{
	u32 dat = (chan & 0x1f) | (data<<6);

	if(REG_IPC_FIFO_CR & (IPC_FIFO_SEND_FULL | IPC_FIFO_ERROR))return -1;
	REG_IPC_FIFO_TX = dat;

	return 0;
}

void bootstrapArm7()
{
	REG_IME = 0;
	REG_IE = 0;
	REG_IF = ~0;

	BOOTFLAG = 0;
	*((vu32*)0x02FFFE04) = (u32)0xE59FF018;
	*((vu32*)0x02FFFE24) = (u32)0x02FFFE04;
	NDSHEADER->arm7executeAddress = 0x06000000;

	while(sendipcmsg(12, 0x10<<8)<0);
	debug_color(31,31,0); // yellow

	bootstrapArm7DSiSync();
	while(IPC_GetSync()!=1);

	debug_color(31,0,31); // magenta

	VRAM_C_CR = VRAM_ENABLE | VRAM_C_LCD;
	memcpy16(VRAM_C, ndsloader_bin, ndsloader_bin_size);
	VRAM_C_CR = VRAM_ENABLE | VRAM_C_ARM7_0x06000000;

	debug_color(0,15,0); // dark green

	IPC_SendSync(1);

	while(IPC_GetSync()==1);

	IPC_SendSync(0);

	debug_color(0,31,0); // green
	while (BOOTFLAG != 42);
	debug_color(15,31,15); // bright green

	swiSoftReset();
	__builtin_unreachable();
}

void bootstrapArm7DSiSync()
{
	vu16 *sync = (vu16*)0x02fffc24;
	vu16 temp;

	sync[2] = 4;
	temp = sync[1];
	while (sync[1]==temp);
	sync[0]++;

	sync[2] = 3; //When this is 3, Arm7 TWL SDK bootstub skips 7i bin loading, otherwise when 2 it loads the 7i bin.
	temp = sync[1];
	while (sync[1]==temp);
	sync[0]++;
}

void* memcpy16(void* a, const void* b, size_t len)
{
	vu16 *bufa = (vu16*)a;
	vu16 *bufb = (vu16*)b;
	while(len>0)
	{
		*bufa = *bufb;
		bufa++;
		bufb++;
		len-=2;
	}
	return a;
}