#ifdef BOOTHB
#include <nds.h>
#include <stdio.h>
#include <string.h>
#endif
#include "common.h"

#include "arm9stub_bin.h"
#include "arm7stub_bin.h"
#ifndef BOOTHB
#include "ndsloader_lz.h"
#endif
//#include "ndsloader_nds.h"

#define ARM9STUBVMA 0x02ffa000

int DSi_mode = 0;

void __attribute__ ((long_call)) resetArm9();
void bootstrapArm7();
void ndsloadstub();
void bootstrapArm7DSiSync();
int sendipcmsg(u32 channel, u32 data);

void* memset16(void* buffer, int val, size_t len);

#ifndef BOOTHB
//#define CAMINIT
#endif

#ifndef BOOTHB
void ipx_caminit();
#endif
#ifndef BOOTHB
#ifdef NANDHAX
void nandread();
void nandread_asm();
#endif
#endif

#ifndef BOOTHB
int main(void)
#else
int generictwlpayload_main(void)
#endif
{
	#ifdef TWL
	if(((*((vu32*)0x04004000)) & 3)==1)DSi_mode = 1;
	#endif

	#ifdef CAMINIT
	ipx_caminit();
	#endif

	#ifndef BOOTHB
	#ifdef NANDHAX
	nandread();
	#endif
	#endif

	#ifndef BOOTHB
	REG_DISPCNT = 0;
	REG_DISPCNT_SUB = 0;
	REG_MASTER_BRIGHT = 0;
	REG_MASTER_BRIGHT_SUB = 0;
	#endif

	resetArm9();
	bootstrapArm7();
	while(1);
	return 0;
}

#ifndef BOOTHB
#ifdef NANDHAX
void nandread()
{
	if(*((u32*)0x02ffe230) != 0x4b344445 && *((u32*)0x02ffe230) != 0x484e4145)return;
	void (*FS_InitCtx)(u8*);
	int (*FS_Open)(u8*, char*, u32);
	int (*FS_Close)(u8*);
	int (*FS_Read)(u8*, u8*, u32);
	u8 ctx[0x48];

	if(*((u32*)0x02ffe230) == 0x4b344445)
	{
		FS_InitCtx = (void*)0x20a1c6c;
		FS_Open = (void*)0x20a1ea4;
		FS_Close = (void*)0x20a1f28;
	}
	else if(*((u32*)0x02ffe230) == 0x484e4145)//launcher. note that this launcher stuff is for on-the-fly patching stuff, no nand-based only hax.
	{
		FS_InitCtx = (void*)0x26baf99;
		FS_Open = (void*)0x26bb4c5;
		FS_Close = (void*)0x26bb511;
		FS_Read = (void*)0x26bb581;
	}

	if(*((u32*)0x02ffe230) == 0x4b344445)
	{
		nandread_asm();
		DC_FlushAll();

		REG_IME = 1;
		FS_InitCtx(ctx);
		if(FS_Open(ctx, "nand:/<tmpjump>", 1)==1)FS_Close(ctx);
		REG_IME = 0;
	}
	else if(*((u32*)0x02ffe230) == 0x484e4145)
	{
		REG_IME = 1;
		FS_InitCtx(ctx);
		if(FS_Open(ctx, "nand:/sys/arm7hax.bin", 1)==1)
		{
			FS_Read(ctx, (void*)0x3780260, 0x40);//this is the beginning of the nand_read_sectors func. since this data won't get written there until after that func returns, another read is triggered below.
			FS_Close(ctx);
			FS_InitCtx(ctx);
			if(FS_Open(ctx, "nand:/<tmpjump>", 1)==1)FS_Close(ctx);//unless the arm7hax code returns,(my local code doesn't) this will hang.
		}
		REG_IME = 0;
	}
}
#endif
#endif

#ifdef BOOTHB
void generictwlpayload_setargv(int argc, char **argv)
{
	u32 *param = (u32*)(BOOTLDR_NDS-0x1000-8);
	char *argv_params = (char*)(BOOTLDR_NDS-0x1000);
	int i, pos = 0;
	if(argc==0)*param = 0;
	if(argc)
	{
		memset(param, 0, 0x1000);
		*param = 0x56475241;
		param[1] = (u32)argc;
		for(i=0; i<argc; i++)
		{
			strncpy(&argv_params[pos], argv[i], 0x1000-9-pos);
			pos+=strlen(argv[i])+1;
		}
	}
}
#endif

void __attribute__ ((long_call)) 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;
		if(DSi_mode)
		{
			for(reg=0; reg<0x1c; reg+=4)*((u32*)(0x04004104 + ((i*0x1c)+reg))) = 0;//Reset NDMA.
		}
	}

	VRAM_CR = (VRAM_CR & 0xffff0000) | 0x00008080;//This is from bootloader.
	
	//This DMA gfx reset code is from bootloader boot.c.
	dmaFillWords( 0, (void*)0x04000000, 0x56);  //clear main display registers
	dmaFillWords( 0, (void*)0x04001000, 0x56);  //clear sub  display registers

	REG_DISPSTAT = 0;
	REG_DISPCNT = 0;
	REG_DISPCNT_SUB = 0;

	for(i=0; i<7; i++)//Clear VRAM.
	{
		if(i==2)continue;
		((vu8*)0x04000240)[i] = 0x80;
	}
	VRAM_H_CR = 0x80;
	VRAM_I_CR = 0x80;
	memset16((void*)0x6800000, 0, 0xa4000);

	for(i=0; i<7; i++)//Reset VRAM.
	{
		if(i==2)continue;
		((vu8*)0x04000240)[i] = 0;
	}
	VRAM_H_CR = 0;
	VRAM_I_CR = 0;

	memset((void*)0x05000000, 0, 0x800);//Clear palettes.
	memset((void*)0x07000000, 0, 0x800);//Clear OAM.
	memset(SPRITE_GFX, 0, 128 * 1024);
        memset(SPRITE_GFX_SUB, 0, 128 * 1024);

	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 = 1;
	#ifndef BOOTHB
	while(sendipcmsg(12, 0x10<<8)<0);
	#endif
	while(REG_VCOUNT!=192);//Wait for Arm7 to enter TWL SDK bootstub, by waiting for a vblank.
	while(REG_VCOUNT==192);
	REG_IME = 0;

	REG_IE = 0;
	REG_IF = ~0;

	#ifndef BOOTHB
	#ifndef TWL
	*((vu32*)0x027e3ffc) = 0;//Disable the IRQ handler.
	#else
	*((vu32*)0x02fe3ffc) = 0;
	#endif
	#endif

	#ifndef BOOTHB
	if(DSi_mode)bootstrapArm7DSiSync();
	while(IPC_GetSync()!=1);
	IPC_SendSync(1);
	#endif

	#ifndef BOOTHB
	while(IPC_GetSync()==1);
	#endif
	ndsloadstub();
}

void ndsloadstub()
{
	void (*func_ptr)(void) = (void*)ARM9STUBVMA;
	#ifndef BOOTHB
	swiDecompressLZSSWram((void*)ndsloader_lz, (void*)BOOTLDR_NDS);
	*((u32*)(BOOTLDR_NDS-8)) = *((u32*)ndsloader_lz) >> 8;
	#endif
	DC_FlushAll();
	DC_InvalidateAll();
	IC_InvalidateAll();

	BOOTFLAG = 0;
	VRAM_C_CR = VRAM_ENABLE | VRAM_C_LCD;
	#ifndef BOOTHB
	NDSHEADER->arm7executeAddress = 0x06000000;
	#endif
	memcpy16((void*)ARM9STUBVMA, arm9stub_bin, arm9stub_bin_size);
	memcpy16(VRAM_C, arm7stub_bin, arm7stub_bin_size);
	VRAM_C_CR = VRAM_ENABLE | VRAM_C_ARM7_0x06000000;

	DC_FlushAll();
	DC_InvalidateAll();
	IC_InvalidateAll();
	#ifdef BOOTHB
	resetARM7(0x06000000);
	#endif
	func_ptr();
}

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

	sync[2] = 3;//When this is 3, Arm7 TWL SDK bootstub skips 7i bin loading, otherwise when 2 it loads the 7i bin.
	//sync[2] = 2;

	temp = sync[1];
	do
	{
		sync[0]++;
		
	} while(sync[1]==temp);

	temp = sync[1];
	do
	{
		sync[0]++;
		
	} while(sync[1]==temp);
}

#ifndef BOOTHB
void* memset(void* buffer, int val, size_t len)
{
	vu32 *buf = (vu32*)buffer;
	while(len>0)
	{
		*buf = val;
		buf++;
		len-=4;
	}
	return buffer;
}
#endif

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;
}

void* memset16(void* buffer, int val, size_t len)
{
	vu16 *buf = (vu16*)buffer;
	while(len>0)
	{
		*buf = val;
		buf++;
		len-=2;
	}
	return buffer;
}