#include <nds.h>
#include "exodecr.h"
#include "ndsloader_bin.h"

extern char __base_addr[];

static void resetArm9(void);
static void bootstrapArm7(void);
static void bootstrapArm7DSiSync(void);
static int sendipcmsg(u32 channel, u32 data);
static void* memcpy16(void* a, const void* b, size_t len);

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(void)
{
	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;

	REG_IE = 0;
	REG_IF = ~0;
}

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(void)
{
	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;
	char* decrbegin = exo_decrunch((char*)ndsloader_bin_end, __base_addr);
	u32 decrsize = __base_addr-decrbegin;
	memcpy16(VRAM_C, decrbegin, decrsize);
	VRAM_C_CR = VRAM_ENABLE | VRAM_C_ARM7_0x06000000;
	*(vu32*)0x2380000 = 0xE51FF004; // ldr pc, =0x06000000
	*(vu32*)0x2380004 = 0x06000000; // (constant for above)

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

	IPC_SendSync(1);

	while(IPC_GetSync()!=0);

	IPC_SendSync(0);

	debug_color(15,31,15); // bright green

	// Enter PassMe loop
	*((vu32*)0x02FFFE04) = (u32)0xE59FF018;
	*((vu32*)0x02FFFE24) = (u32)0x02FFFE04;
	((void(*)(void))0x02FFFE04)();
	__builtin_unreachable();
}

void bootstrapArm7DSiSync(void)
{
	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;
}