#include <tamtypes.h>
#include <kernel.h>
#include <sifrpc.h>
#include <fileio.h>
//#include <libmc.h>
//#include <sys/stat.h>
//#include <sys/fcntl.h>
//#include <fileXio_rpc.h>
#include <sifrpc.h>
#include <iopcontrol.h>
#include <iopheap.h>
#include <sbv_patches.h>
#include <loadfile.h>
#include <libpad.h>
#include <string.h>
#include <stdio.h>
#include <osd_config.h>
#include <libcdvd.h>
#include <debug.h>

#define MAX_PATH 260
extern void usbd_irx;
extern int  size_usbd_irx;
extern void usb_mass_irx;
extern int  size_usb_mass_irx;

extern void iomanx_irx;
extern int  size_iomanx_irx;
extern void filexio_irx;
extern int  size_filexio_irx;

extern void init_irx;
extern int  size_init_irx;

#define DEBUG

#define RGBA(r, g, b, a)    \
        ((u64)(r)|((u64)(g) << 8)|((u64)(b) << 16)|((u64)(a) << 24))
        
#ifdef DEBUG
#define SETBG(r, g, b) *(u64*)0x120000e0 = RGBA(r, g, b, 0)
#else
#define SETBG(...)
#endif


char rescue_mass_boot[] = "mass:/RESCUE.ELF";
char rescue_mc_boot[] = "mc0:/RESCUE.ELF";
char mass_boot[] = "mass:/BOOT/BOOT.ELF";
char mc_boot[] = "mc0:/BOOT/BOOT.ELF";
char mc_boot_b[] = "mc0:/B?DATA-SYSTEM/BOOT.ELF";
char mass_boot_R1[] = "mass:/BOOT/BOOT1.ELF";
char mass_boot_R2[] = "mass:/BOOT/BOOT2.ELF";
char mass_boot_L1[] = "mass:/BOOT/BOOT3.ELF";
char mass_boot_L2[] = "mass:/BOOT/BOOT4.ELF";
char mc_boot_R1[] = "mc0:/BOOT/BOOT1.ELF";
char mc_boot_R2[] = "mc0:/BOOT/BOOT2.ELF";
char mc_boot_L1[] = "mc0:/BOOT/BOOT3.ELF";
char mc_boot_L2[] = "mc0:/BOOT/BOOT4.ELF";
char mc_boot_b_R1[] = "mc0:/B?DATA-SYSTEM/BOOT1.ELF";
char mc_boot_b_R2[] = "mc0:/B?DATA-SYSTEM/BOOT2.ELF";
char mc_boot_b_L1[] = "mc0:/B?DATA-SYSTEM/BOOT3.ELF";
char mc_boot_b_L2[] = "mc0:/B?DATA-SYSTEM/BOOT4.ELF";

char cnf_path[MAX_PATH] = "mc0:/FMCB-CNF/FREEMCB.CNF";

char cdboot_path[MAX_PATH];

int Skip_MC = 1;
int Skip_HDD = 1;
int Debug_Screen = 0;
int AutoBoot_Disc = 0;


char LK_E1_Path[17][MAX_PATH];
char LK_E2_Path[17][MAX_PATH];
char LK_E3_Path[17][MAX_PATH];

char LK_ID[17][10] = {
	"Auto",
	"Circle",
	"Cross",
	"Square",
	"Triangle",
	"L1",
	"R1",
	"L2",
	"R2",
	"L3",
	"R3",
	"Up",
	"Down",
	"Left",
	"Right",
	"Start",
	"Select"
};


//--------------------------------------------------------------
void wipeUserMem(void)
{
  int i;
  for (i = 0x100000; i < 0x2000000 ; i += 64) {
  asm (
    "\tsq $0, 0(%0) \n"
    "\tsq $0, 16(%0) \n"
    "\tsq $0, 32(%0) \n"
    "\tsq $0, 48(%0) \n"
    :: "r" (i) );
  }
}



//________________ From uLaunchELF ______________________

static char padBuf_t[2][256] __attribute__((aligned(64)));
struct padButtonStatus buttons_t[2];
u32 padtype_t[2];
u32 paddata, paddata_t[2];
u32 old_pad = 0, old_pad_t[2] = {0, 0};
u32 new_pad, new_pad_t[2];
u32 joy_value = 0;
static int test_joy = 0;

#define PAD_R3_V0 0x010000
#define PAD_R3_V1 0x020000
#define PAD_R3_H0 0x040000
#define PAD_R3_H1 0x080000
#define PAD_L3_V0 0x100000
#define PAD_L3_V1 0x200000
#define PAD_L3_H0 0x400000
#define PAD_L3_H1 0x800000

//----------------------------------------------------------------
int readPad(void)
{
	static int n[2]={0,0}, nn[2]={0,0};
	int port, state, ret[2];

	for(port=0; port<2; port++){
		if((state=padGetState(port, 0))==PAD_STATE_STABLE
			||(state == PAD_STATE_FINDCTP1)){
			//Deal with cases where pad state is valid for padRead
			ret[port] = padRead(port, 0, &buttons_t[port]);
			if (ret[port] != 0){
				paddata_t[port] = 0xffff ^ buttons_t[port].btns;
				if((padtype_t[port] == 2) && (1 & (test_joy++))){//DualShock && time for joy scan
					joy_value=0;
					if(buttons_t[port].rjoy_h >= 0xbf){
						paddata_t[port]=PAD_R3_H1;
						joy_value=buttons_t[port].rjoy_h-0xbf;
					}else if(buttons_t[port].rjoy_h <= 0x40){
						paddata_t[port]=PAD_R3_H0;
						joy_value=-(buttons_t[port].rjoy_h-0x40);
					}else if(buttons_t[port].rjoy_v <= 0x40){
						paddata_t[port]=PAD_R3_V0;
						joy_value=-(buttons_t[port].rjoy_v-0x40);
					}else if(buttons_t[port].rjoy_v >= 0xbf){
						paddata_t[port]=PAD_R3_V1;
						joy_value=buttons_t[port].rjoy_v-0xbf;
					}else if(buttons_t[port].ljoy_h >= 0xbf){
						paddata_t[port]=PAD_L3_H1;
						joy_value=buttons_t[port].ljoy_h-0xbf;
					}else if(buttons_t[port].ljoy_h <= 0x40){
						paddata_t[port]=PAD_L3_H0;
						joy_value=-(buttons_t[port].ljoy_h-0x40);
					}else if(buttons_t[port].ljoy_v <= 0x40){
						paddata_t[port]=PAD_L3_V0;
						joy_value=-(buttons_t[port].ljoy_v-0x40);
					}else if(buttons_t[port].ljoy_v >= 0xbf){
						paddata_t[port]=PAD_L3_V1;
						joy_value=buttons_t[port].ljoy_v-0xbf;
					}
				}
				new_pad_t[port] = paddata_t[port] & ~old_pad_t[port];
				if(old_pad_t[port]==paddata_t[port]){
					n[port]++;
					/*
					if(gsKit_detect_signal()==GS_MODE_NTSC){
						if(n[port]>=25){
							new_pad_t[port]=paddata_t[port];
							if(nn[port]++ < 20)	n[port]=20;
							else			n[port]=23;
						}
					}else{
						if(n[port]>=21){
							new_pad_t[port]=paddata_t[port];
							if(nn[port]++ < 20)	n[port]=17;
							else			n[port]=19;
						}
					}
					*/
				}else{
					n[port]=0;
					nn[port]=0;
					old_pad_t[port] = paddata_t[port];
				}
			}
		}else{
			//Deal with cases where pad state is not valid for padRead
			new_pad_t[port]=0;
		}  //ends 'if' testing for state valid for padRead
	}  //ends for
	new_pad = new_pad_t[0]|new_pad_t[1];
	return (ret[0]|ret[1]);
}
//----------------------------------------------------------------
// Wait for specific PAD, but also accept disconnected state
void waitPadReady(int port, int slot)
{
	int state, lastState;
	char stateString[16];

	state = padGetState(port, slot);
	lastState = -1;
	while((state != PAD_STATE_DISCONN)
		&& (state != PAD_STATE_STABLE)
		&& (state != PAD_STATE_FINDCTP1)){
		if (state != lastState)
			padStateInt2String(state, stateString);
		lastState = state;
		state=padGetState(port, slot);
	}
}
//---------------------------------------------------------------------------
// Wait for any PAD, but also accept disconnected states
void waitAnyPadReady(void)
{
	int state_1, state_2;

	state_1 = padGetState(0, 0);
	state_2 = padGetState(1, 0);
	while((state_1 != PAD_STATE_DISCONN) && (state_2 != PAD_STATE_DISCONN)
		&& (state_1 != PAD_STATE_STABLE) && (state_2 != PAD_STATE_STABLE)
		&& (state_1 != PAD_STATE_FINDCTP1) && (state_2 != PAD_STATE_FINDCTP1)){
		state_1 = padGetState(0, 0);
		state_2 = padGetState(1, 0);
	}
}
//----------------------------------------------------------------
// setup PAD
int setupPad(void)
{
	int ret, i, port, state, modes;

	padInit(0);

	for(port=0; port<2; port++){
		padtype_t[port] = 0;  //Assume that we don't have a proper PS2 controller
		if((ret = padPortOpen(port, 0, &padBuf_t[port][0])) == 0)
			return 0;
		waitPadReady(port, 0);
		state = padGetState(port, 0);
		if(state != PAD_STATE_DISCONN){ //if anything connected to this port
			modes = padInfoMode(port, 0, PAD_MODETABLE, -1);
			if (modes != 0){ //modes != 0, so it may be a dualshock type
				for(i=0; i<modes; i++){
					if (padInfoMode(port, 0, PAD_MODETABLE, i) == PAD_TYPE_DUALSHOCK){
						padtype_t[port] = 2; //flag normal PS2 controller
						break;
					}
				} //ends for (modes)
			} else { //modes == 0, so this is a digital controller
				padtype_t[port] = 1; //flag digital controller
			}
			if(padtype_t[port] == 2)                                        //if DualShock
				padSetMainMode(port, 0, PAD_MMODE_DUALSHOCK, PAD_MMODE_LOCK);   //Set DualShock
			else                                                            //else
				padSetMainMode(port, 0, PAD_MMODE_DIGITAL, PAD_MMODE_UNLOCK);   //Set Digital
			waitPadReady(port, 0);                                          //Await completion
		} else {                                          //Nothing is connected to this port
				padSetMainMode(port, 0, PAD_MMODE_DUALSHOCK, PAD_MMODE_LOCK); //Fake DualShock
				waitPadReady(port, 0);                                        //Await completion
		}
	} //ends for (port)
	return 1;
}

//________________ From uLaunchELF ______________________
// CNF Parser
//----------------------------------------------------------------
int get_CNF_string(unsigned char **CNF_p_p,
                   unsigned char **name_p_p,
                   unsigned char **value_p_p)
{
	unsigned char *np, *vp, *tp = *CNF_p_p;

start_line:
	while ((*tp <= ' ') && (*tp > '\0')) tp += 1;                       // Skip leading whitespace, if any
	if (*tp == '\0') return 0;                                      	// but exit at EOF
	np = tp;                                                            // Current pos is potential name
	if (*tp < 'A')                                                      // but may be a comment line
	{                                                                   // We must skip a comment line
		while ((*tp != '\r') && (*tp != '\n') && (*tp > '\0')) tp += 1; // Seek line end
		goto start_line;                                                // Go back to try next line
	}

	while ((*tp >= 'A') || ((*tp >= '0') && (*tp <= '9'))) tp += 1;     // Seek name end
	if (*tp == '\0') return 0;                                      	// but exit at EOF
	*tp++ = '\0';                                                       // terminate name string (passing)
	while ((*tp <= ' ') && (*tp > '\0')) tp += 1;                       // Skip post-name whitespace, if any
	if (*tp != '=') return 0;                                       	// exit (syntax error) if '=' missing
	tp += 1;                                                            // skip '='
	while ((*tp <= ' ') && (*tp > '\0')                                 // Skip pre-value whitespace, if any
		&& (*tp != '\r') && (*tp != '\n')) tp += 1;                     // but do not pass the end of the line
	if (*tp == '\0') return 0;                                      	// but exit at EOF
	vp = tp;                                                            // Current pos is potential value

	while ((*tp != '\r') && (*tp != '\n') && (*tp != '\0')) tp += 1;    // Seek line end
	if (*tp != '\0') *tp++ = '\0';                                      // terminate value (passing if not EOF)
	while ((*tp <= ' ') && (*tp > '\0')) tp += 1;                       // Skip following whitespace, if any

	*CNF_p_p = tp;                                                      // return new CNF file position
	*name_p_p = np;                                                     // return found variable name
	*value_p_p = vp;                                                    // return found variable value
	return 1;                                                        	// return control to caller
} // Ends get_CNF_string
//----------------------------------------------------------------
// Load CNF
//----------------------------------------------------------------
int loadConfig(void)
{
	int i, fd, var_cnt, CNF_version;
	size_t CNF_size;
	char tsts[20];
	char path[MAX_PATH];
	unsigned char *RAM_p, *CNF_p, *name, *value;

	strcpy(path, cnf_path);	
	fd = -1;
	fd = open(path, O_RDONLY);
	if (fd < 0) {
		path[2] = '1';
		fd = open(path, O_RDONLY);
	}
	if (fd < 0) {
failed_load:
		return -1;
	} // This point is only reached after succefully opening CNF

	CNF_size = lseek(fd, 0, SEEK_END);
	lseek(fd, 0, SEEK_SET);
	RAM_p = (char*)malloc(CNF_size);
	CNF_p = RAM_p;
	if (CNF_p == NULL) {
		close(fd);
		goto failed_load;
	}
	read(fd, CNF_p, CNF_size); // Read CNF as one long string
	close(fd);
	CNF_p[CNF_size] = '\0'; // Terminate the CNF string

	CNF_version = 0; // The CNF version is still unidentified
	for (var_cnt = 0; get_CNF_string(&CNF_p, &name, &value); var_cnt++) {
		// A variable was found, now we dispose of its value.
		if (!strcmp(name,"CNF_version")) {
			CNF_version = atoi(value);
			continue;
		} else if (CNF_version == 0) {
			goto failed_load; // Refuse unidentified CNF
		}
		if (!strcmp(name,"AutoBoot_Disc")) {
			AutoBoot_Disc = atoi(value);;
			continue;
		}		
		if (!strcmp(name,"Skip_MC")) {
			Skip_MC = atoi(value);;
			continue;
		}		
		if (!strcmp(name,"Skip_HDD")) {
			Skip_HDD = atoi(value);;
			continue;
		}		
		if (!strcmp(name,"Debug_Screen")) {
			Debug_Screen = atoi(value);;
			continue;
		}		
		for (i=0; i<17; i++) {
			sprintf(tsts, "LK_%s_E1", LK_ID[i]);
			if (!strcmp(name, tsts)) {
				strcpy(LK_E1_Path[i], value);
				//scr_printf("%s\n", LK_E1_Path[i]);
				break;
			}
		}
		for (i=0; i<17; i++) {
			sprintf(tsts, "LK_%s_E2", LK_ID[i]);
			if (!strcmp(name, tsts)) {
				strcpy(LK_E2_Path[i], value);
				//scr_printf("%s\n", LK_E2_Path[i]);
				break;
			}
		}
		for (i=0; i<17; i++) {
			sprintf(tsts, "LK_%s_E3", LK_ID[i]);
			if (!strcmp(name, tsts)) {
				strcpy(LK_E3_Path[i], value);
				//scr_printf("%s\n", LK_E3_Path[i]);
				break;
			}
		}
	} // ends for
	free(RAM_p);
	return 0;
}
//--------------------------------------------------------------
// Read SYSTEM.CNF for MISC/PS2Disc launch command
int ReadCNF(char *LK_Path)
{
	char *systemcnf;
	int fd;
	int size;
	int n;
	int i;
	
	cdInit(CDVD_INIT_INIT);

	i = 0x10000;
	while(i--) asm("nop\nnop\nnop\nnop");
	fd = fioOpen("cdrom0:\\SYSTEM.CNF;1",1);
	if(fd>=0) {
		size = fioLseek(fd,0,SEEK_END);
		fioLseek(fd,0,SEEK_SET);
		systemcnf = (char*)malloc(size+1);
		fioRead(fd, systemcnf, size);
		systemcnf[size+1]=0;
		for(n=0; systemcnf[n]!=0; n++){
			if(!strncmp(&systemcnf[n], "BOOT2", 5)) {
				n+=5;
				break;
			}
		}
		while(systemcnf[n]!=0 && systemcnf[n]==' ') n++;
		if(systemcnf[n]!=0 ) n++; // salta '='
		while(systemcnf[n]!=0 && systemcnf[n]==' ') n++;
		if(systemcnf[n]==0){
			free(systemcnf);
			return 0;
		}
		
		for(i=0; systemcnf[n+i]!=0; i++) {
			LK_Path[i] = systemcnf[n+i];
			if(i>2)
				if(!strncmp(&LK_Path[i-1], ";1", 2)) {
					LK_Path[i+1]=0;
					break;
				}
		}
		fioClose(fd);
		free(systemcnf);
		return 1;
	}
	return 0;
}


//________________ From uLaunchELF ______________________
// Timer Define
#define T0_COUNT ((volatile unsigned long*)0x10000000) 
#define T0_MODE  ((volatile unsigned long*)0x10000010) 

static int TimerInterruptID = -1;
static u64 TimerInterruptCount = 0;

//--------------------------------------------------------------
// Timer Interrupt
int TimerInterrupt(int a)
{
	TimerInterruptCount++;
	*T0_MODE |= (1 << 11);
	return -1;
}
//--------------------------------------------------------------
// Timer Init
void TimerInit(void)
{
	*T0_MODE = 0x0000;
	TimerInterruptID = AddIntcHandler(9, TimerInterrupt, 0);
	EnableIntc(9);
	*T0_COUNT = 0;
	*T0_MODE = ( 2 << 0 )+( 1 << 7 )+( 1 << 9 );
	TimerInterruptCount = 0;
}
//--------------------------------------------------------------
// Timer Count
u64 Timer(void)
{
	u64 ticks = (*T0_COUNT + (TimerInterruptCount << 16)) * (1000.0F / ( 147456000.0F / 256.0F ));
	return ticks;
}
//--------------------------------------------------------------
// Timer End
void TimerEnd(void)
{
	*T0_MODE = 0x0000;
	if (TimerInterruptID >= 0){
		DisableIntc(9);
		RemoveIntcHandler(9, TimerInterruptID);
		TimerInterruptID = -1;
	}
	TimerInterruptCount = 0;
}
//--------------------------------------------------------------



//________________ Neme's OSDSYS loading & hacking code ______________________

void patch_menu(u8 *osd);
u8 *find_bytes_with_mask(u8 *buf, u32 bufsize, u8 *bytes, u8 *mask, u32 len);
void launch_ps2disc(void);

#define NEWITEMS	1		// the number of added menu items
#define OSD_MAGIC	0x39390000	// arbitrary number to identify added menu items

static u32 osdmenu[4 + NEWITEMS * 2];

// Define the new items here.
// Then define their behaviour in the handle_menu_selection() function.
static const char *menuitems[NEWITEMS] = {
  //"Launch PS2Disc",
  "Launch Free McBoot"
};

static u32 execps2_code[] = {
  0x24030007,			// li v1, 7
  0x0000000c,			// syscall
  0x03e00008,			// jr ra
  0x00000000			// nop
};
static u32 execps2_mask[] = {
  0xffffffff,
  0xffffffff,
  0xffffffff,
  0xffffffff
};

static u32 pattern1[] = {
  0x00000001,			// unknown
  0x00000000,			// pointer to osdmenu
  0x00000002,			// number of entries
  0x00000003,			// unknown
  0x00000000			// current selection
};
static u32 pattern1_mask[] = {
  0xffffffff,
  0xff800000,
  0xffffffff,
  0xffffffff,
  0xffffffff
};

static u32 pattern2[] = {	// Search pattern in the osd string function:
  0x10000005,			//     beq	zero, zero, L2
  0x8c620000,			//     lw	v0, $xxxx(v1)
  0x00101880,			// L1: sll	v1, s0, 2	# string index * 4
  0x8c440000,			//     lw	a0, $xxxx(v0)	# osd string pointer array
  0x00641821,			//     addu	v1, v1, a0	# byte offset into array
  0x8c620000,			//     lw	v0, $0000(v1)	# pointer to string
  0xdfbf0010			// L2: ld	ra, $0010(sp)
};
static u32 pattern2_mask[] = {
  0xffffffff,
  0xffff0000,
  0xffffffff,
  0xffff0000,
  0xffffffff,
  0xffffffff,
  0xffffffff
};

static u32 pattern3[] = {	// Search pattern in the user input handling function:
  0x1000000e,			//     beq	zero, zero, exit
  0xdfbf0010,			//     ld	ra, $0010(sp)
  0x24040001,			// L1: li	a0, 1		# the 2nd menu item (sys conf)
  0x8c430000,			//     lw	v1, $xxxx(v0)	# current selection
  0x1464000a,			//     bne	v1, a0, exit	# sys config not selected?
  0xdfbf0010,			//     ld	ra, $0010(sp)
  0x0c000000,			//     jal	StartSysConfig
  0x00000000,			//     nop
  0x10000006,			//     beq	zero, zero, exit
  0xdfbf0010			//     ld	ra, $0010(sp)
};
static u32 pattern3_mask[] = {
  0xffffffff,
  0xffffffff,
  0xffffffff,
  0xffff0000,
  0xffffffff,
  0xffffffff,
  0xfc000000,
  0xffffffff,
  0xffffffff,
  0xffffffff
};


//--------------------------------------------------------------
u8 *find_bytes_with_mask(u8 *buf, u32 bufsize, u8 *bytes, u8 *mask, u32 len)
{
  u32 i, j;

  for (i = 0; i < bufsize - len; i++) {
    for (j = 0; j < len; j++) {
      if ((buf[i + j] & mask[j]) != bytes[j])
        break;
    }
    if (j == len)
      return &buf[i];
  }
  return NULL;
}
//--------------------------------------------------------------
u8 *find_string(const u8 *string, u8 *buf, u32 bufsize)
{
  u32 i;
  const u8 *s, *p;

  for (i = 0; i < bufsize; i++) {
    s = string;
    for (p = buf + i; *s && *s == *p; s++, p++);
    if (!*s)
      return (buf + i);
  }
  return NULL;
}
//--------------------------------------------------------------
const char *get_string_pointer(const char **strings, u32 index)
{
  if ((index & 0xffff0000) == OSD_MAGIC)
    return menuitems[index & 0xffff];

  return strings[index];
}
//--------------------------------------------------------------
int handle_menu_selection(int selected)
{
  char *args[2];

  if (selected == 1)
    return 1;
  if (selected >= 2 + NEWITEMS)
    return 0;

  switch (selected - 2) {
/*    case 0:					// Boot ps2 disc
      args[0] = "rom0:";
      args[1] = "ExecutePs2GameDisk";
      LoadExecPS2("rom0:OSDSYS", 2, args);
      break;
*/
    case 0:					// Boot MC
      args[0] = "rom0:";
      args[1] = "BootBrowser";
      LoadExecPS2("rom0:OSDSYS", 2, args);
      break;
  }

  return 0;
}
//--------------------------------------------------------------
void patch_menu(u8 *osd)
{
  u8 *ptr;
  u32 tmp, menu_addr, p2_addr, p3_addr, i;

  //------------------------------------------------------------------------
  // Search for all patterns and return if one of them not found
  //------------------------------------------------------------------------
  for (tmp = 0; tmp < 0x100000; tmp = (u32)(ptr - osd + 4)) {
    ptr = find_bytes_with_mask(osd + tmp, 0x100000 - tmp, (u8*)pattern1, (u8*)pattern1_mask, sizeof(pattern1));
    if (!ptr)
      return;
    if (_lw((u32)ptr + 4) == (u32)ptr - 4*4)
      break;
  }
  menu_addr = (u32)ptr;

  ptr = find_bytes_with_mask(osd, 0x00100000, (u8*)pattern2, (u8*)pattern2_mask, sizeof(pattern2));
  if (!ptr)
    return;
  p2_addr = (u32)ptr;

  ptr = find_bytes_with_mask(osd, 0x00100000, (u8*)pattern3, (u8*)pattern3_mask, sizeof(pattern3));
  if (!ptr)
    return;
  p3_addr = (u32)ptr;


  //------------------------------------------------------------------------
  // Patch the osd string function
  //------------------------------------------------------------------------
  tmp = 0x0c000000;
  tmp |= ((u32)get_string_pointer >> 2);
  _sw(0x0200282d, p2_addr + 2*4);		// daddu  a1, s0, zero
						// lw     a0, $xxxx(v0)
  _sw(tmp,        p2_addr + 4*4);		// jal    get_string_pointer
  _sw(0x00000000, p2_addr + 5*4);		// nop


  //------------------------------------------------------------------------
  // Patch the user input handling function
  //------------------------------------------------------------------------
  tmp = 0x0c000000;
  tmp |= ((u32)handle_menu_selection >> 2);
  _sw(tmp,        p3_addr + 2*4);		// jal    handle_menu_selection
  tmp = _lw(p3_addr + 3*4) & 0xffff;
  tmp |= 0x8c440000;
  _sw(tmp,        p3_addr + 3*4);		// lw     a0, $xxxx(v0)
  _sw(0x1040000a, p3_addr + 4*4);		// beq    v0, zero, exit


  //------------------------------------------------------------------------
  // Build the osd menu
  //------------------------------------------------------------------------
  osdmenu[0] = _lw(menu_addr - 4*4);		// "Browser"
  osdmenu[1] = _lw(menu_addr - 3*4);
  osdmenu[2] = _lw(menu_addr - 2*4);		// "System Configuration"
  osdmenu[3] = _lw(menu_addr - 1*4);

  for (i = 0; i < NEWITEMS; i++) {
    osdmenu[4 + i*2] = OSD_MAGIC + i;
    osdmenu[5 + i*2] = 0;
  }

  _sw((u32)osdmenu, menu_addr + 4);		// store menu pointer
  _sw(2 + NEWITEMS, menu_addr + 8);		// store number of menu items
}

//--------------------------------------------------------------
void patch_and_execute(void *epc, void *gp)
{
  int n = 0;
  char *args[3], *ptr;

  patch_menu((u8*)epc);

  while ((ptr = find_string("EXEC-SYSTEM", (u8*)epc, 0x100000)))
    strncpy(ptr, "EXEC-OSDSYS", 11);

  args[n++] = "rom0:";

  if (Skip_MC)   
  	if (find_string("SkipMc", (u8*)epc, 0x100000))
    	args[n++] = "SkipMc";

  if (Skip_HDD) 
  	if (find_string("SkipHdd", (u8*)epc, 0x100000))
	    args[n++] = "SkipHdd";

  FlushCache(0);
  FlushCache(2);
  ExecPS2(epc, gp, n, args);
}
//--------------------------------------------------------------
void launch_browser(void)
{
  u8 *ptr;
  t_ExecData exec;

  SifLoadElf("rom0:OSDSYS", &exec);

  if (exec.epc > 0) {

    // If it loaded to 0x200000 it's probably not packed (old osdsys like jap v1).
    // In this case just patch and execute it.
    if ((exec.epc & 0xfff00000) == 0x00200000)
      patch_and_execute((void *)exec.epc, (void *)exec.gp);

    // Find the ExecPS2 function in the unpacker starting from 0x100000.
    ptr = find_bytes_with_mask((u8*)0x100000, 0x1000, (u8*)execps2_code, (u8*)execps2_mask, sizeof(execps2_code));

    // If found replace it with a call to our patch_and_execute() function.
    if (ptr) {
      u32 instr = 0x0c000000;
      instr |= ((u32)patch_and_execute >> 2);
      *(u32*)ptr = instr;
      *(u32*)&ptr[4] = 0;
    }
    
    SifLoadFileExit();
    SifExitRpc();
    while (!SifIopReset("rom0:UDNL rom0:EELOADCNF", 0));
    while (!SifIopSync());

    // Execute the osd unpacker. If the above patching was successful it will
    // call the patch_and_execute() function after unpacking.
    ExecPS2((void *)exec.epc, (void *)exec.gp, 0, NULL);
  }
 
}
//--------------------------------------------------------------

u8 romver[16];
char *argv[1];
int i;
int fdn = 0;
int ret;
char *p;
int lang, r;
u8 *execPathPoint, *execPathPoint2, *execPathPoint3;
t_ExecData exd;
u64 WaitTime;
int boot_disk;
int pad_press = 0;


void check_LK_path (void)
{
	int fdn = 0;
	
	if (!strcmp(execPathPoint, "OSDSYS")) launch_browser(); // if LK_?_E1 entry is set to OSDSYS then launch browser
	if (!strncmp(execPathPoint + 5, "B?DATA-SYTEM", 12)) memset(execPathPoint + 6, mc_boot_b[6], 1); // Check if LK_?_E? entry path start with mc?:/B?DATA-SYSTEM										
	if (!strncmp(execPathPoint, "mc?:", 4))  // Check if LK_?_E? entry path start with mc?: in this case search for the file in 2 slots.
	{   
		memset(execPathPoint + 2, 0x30, 1);  
		if ((fdn = open(execPathPoint, O_RDONLY)) <= 0) memset(execPathPoint + 2, 0x31, 1);
		close(fdn);
	}	
}

//--------------------------------------------------------------


int _start()
{
  wipeUserMem();
  //clearing mem, so better not to have anything valuable on stack
  SifInitRpc(0);


  while(!SifIopReset("rom0:UDNL rom0:EELOADCNF",0));
  while(!SifIopSync());
  fioExit();
  SifExitIopHeap();
  SifLoadFileExit();
  SifExitRpc();
  SifExitCmd();

  SifInitRpc(0);
  FlushCache(0);
  FlushCache(2);
  //twice, some in-hdloader hack
  while(!SifIopReset("rom0:UDNL rom0:EELOADCNF",0));
  while(!SifIopSync());
  fioExit();
  SifExitIopHeap();
  SifLoadFileExit();
  SifExitRpc();
  SifExitCmd();

  SifInitRpc(0);
  FlushCache(0);
  FlushCache(2);

  SifLoadFileInit();
  sbv_patch_enable_lmb();
  sbv_patch_disable_prefix_check();

  SifExecModuleBuffer(&iomanx_irx, size_iomanx_irx, 0, NULL, &ret);

  SifLoadModule("rom0:SIO2MAN", 0, 0);
  SifLoadModule("rom0:CDVDFSV", 0, 0);  
  SifLoadModule("rom0:CDVDMAN", 0, 0);
  SifLoadModule("rom0:MCMAN", 0, 0);
  SifLoadModule("rom0:MCSERV", 0, 0);
  SifLoadModule("rom0:PADMAN", 0, 0);  
  
  SifExecModuleBuffer(&usbd_irx, size_usbd_irx, 0, NULL, &ret);
  SifExecModuleBuffer(&usb_mass_irx, size_usb_mass_irx, 0, NULL, &ret);
  SifExecModuleBuffer(&init_irx, size_init_irx, 0, NULL, &ret);

  SifLoadFileExit();
  lang = 0x07814233; //Default english PAL - just in case.
  SetOsdConfigParam(&lang);
  lang  = *(vs32*)0x20100000;
  
  configSetLanguage(lang);
  
  if((fdn = open("rom0:ROMVER", O_RDONLY)) > 0)
  {
    read(fdn, romver, sizeof romver);
    close(fdn);
  }
  //mc_boot_b[6] = (romver[4] == 'E' ? 'E' :
  //      (romver[4] == 'J' ? 'I' : 'A'));
  mc_boot_b[6] = mc_boot_b_R1[6] = mc_boot_b_R2[6] = mc_boot_b_L1[6] = mc_boot_b_L2[6] = (romver[4] == 'E' ? 'E' :
        (romver[4] == 'J' ? 'I' : 
        	 (romver[4] == 'U' ? 'A' : romver[4])));
  
        	 
  //padReset();      
  setupPad();
  waitAnyPadReady();
  
  TimerInit();
  
  //init_scr();
  
  
  // Test here for rescue boot files
  execPathPoint = rescue_mass_boot;
  if ((fdn = open(rescue_mass_boot, O_RDONLY)) <= 0) 
  {
  		execPathPoint = rescue_mc_boot;
  		if ((fdn = open(rescue_mc_boot, O_RDONLY)) <= 0) 
  		{
  			rescue_mc_boot[2]++;
  			execPathPoint = rescue_mc_boot;
  			if ((fdn = open(rescue_mc_boot, O_RDONLY)) <= 0)
			{	  			

      			if (loadConfig() == 0) 
  				{
  	    			// Config file found, custom boot way
  	    			boot_disk = 0;
  	    
 	  				execPathPoint  = LK_E1_Path[0]; //setting auto path 1
 	  				execPathPoint2 = LK_E2_Path[0]; //setting auto path 2
 	  				execPathPoint3 = LK_E3_Path[0]; //setting auto path 3
 	  
					WaitTime = Timer();
					while (1) 
					{
						waitAnyPadReady();
						if (readPad()) 
						{
							if (new_pad & PAD_CIRCLE) { execPathPoint = LK_E1_Path[1]; execPathPoint2 = LK_E2_Path[1]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_CROSS) { execPathPoint = LK_E1_Path[2]; execPathPoint2 = LK_E2_Path[2]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_SQUARE) { execPathPoint = LK_E1_Path[3]; execPathPoint2 = LK_E2_Path[3];  execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_TRIANGLE) { execPathPoint = LK_E1_Path[4]; execPathPoint2 = LK_E2_Path[4]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_L1) { execPathPoint = LK_E1_Path[5]; execPathPoint2 = LK_E2_Path[5]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_R1) { execPathPoint = LK_E1_Path[6]; execPathPoint2 = LK_E2_Path[6]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_L2) { execPathPoint = LK_E1_Path[7]; execPathPoint2 = LK_E2_Path[7]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_R2) { execPathPoint = LK_E1_Path[8]; execPathPoint2 = LK_E2_Path[8]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_L3) { execPathPoint = LK_E1_Path[9]; execPathPoint2 = LK_E2_Path[9]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_R3) { execPathPoint = LK_E1_Path[10]; execPathPoint2 = LK_E2_Path[10]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_UP) { execPathPoint = LK_E1_Path[11]; execPathPoint2 = LK_E2_Path[11]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_DOWN)	{ execPathPoint = LK_E1_Path[12]; execPathPoint2 = LK_E2_Path[12]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_LEFT) { execPathPoint = LK_E1_Path[13]; execPathPoint2 = LK_E2_Path[13]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_RIGHT) { execPathPoint = LK_E1_Path[14]; execPathPoint2 = LK_E2_Path[14]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_START) { execPathPoint = LK_E1_Path[15]; execPathPoint2 = LK_E2_Path[15]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							if (new_pad & PAD_SELECT) { execPathPoint = LK_E1_Path[16]; execPathPoint2 = LK_E2_Path[16]; execPathPoint3 = LK_E3_Path[1]; pad_press = 1; }
							//goto test_boot_file;
						}
						if ((Timer() > (WaitTime + 3000)) || (pad_press)) goto test_boot_file; // wait pad button during 3 sec.
					}
		
test_boot_file:

					if ((!strcmp(execPathPoint, LK_E1_Path[0])) && AutoBoot_Disc) // No key have been pressed && AutoBoot_Disk = 1
						if (ReadCNF(cdboot_path)) { execPathPoint = cdboot_path; boot_disk = 1; } //If SYSTEM.CNF is present then boot file is disk main elf   
					
					
					check_LK_path();
					if (((fdn = open(execPathPoint, O_RDONLY)) <= 0) && (!(boot_disk))) execPathPoint = execPathPoint2;
					close(fdn);
					
					check_LK_path();
					if (((fdn = open(execPathPoint, O_RDONLY)) <= 0) && (!(boot_disk))) execPathPoint = execPathPoint3;
					close(fdn);
					
					check_LK_path();
										
					if ((fdn = open(execPathPoint, O_RDONLY)) <= 0) // check if boot file exists
					{
						if (!(Debug_Screen)) launch_browser(); //If Debug_Screen is set to 0 and no boot file found then launch browser
						SETBG(0, 0xff, 0); // File not found, Green screen
						while (1) {;}
					}
					close(fdn);
		
  				}
  				else
  				{
	   				// No Config file found, default boot way	  
					WaitTime = Timer();
  					//printf("awaiting keypress...\n"); 
  					while(1)
  					{
	  					waitAnyPadReady();
  						if(readPad())
  						{
  							if(new_pad & PAD_R1)
  							{
   								strcpy(mass_boot, mass_boot_R1);	  		
   								strcpy(mc_boot, mc_boot_R1);
   								strcpy(mc_boot_b, mc_boot_b_R1);
   								pad_press = 1;
  							}
  							else if(new_pad & PAD_R2)
  							{
   								strcpy(mass_boot, mass_boot_R2);	  			  		
   								strcpy(mc_boot, mc_boot_R2);
   								strcpy(mc_boot_b, mc_boot_b_R2);	      	
   								pad_press = 1;
  							}
  							else if(new_pad & PAD_L1)
  							{
   								strcpy(mass_boot, mass_boot_L1);	  			  		
   								strcpy(mc_boot, mc_boot_L1);
   								strcpy(mc_boot_b, mc_boot_b_L1);	      	
   								pad_press = 1;
  							}
  							else if(new_pad & PAD_L2)
  							{
   								strcpy(mass_boot, mass_boot_L2);	  			  		
   								strcpy(mc_boot, mc_boot_L2);
   								strcpy(mc_boot_b, mc_boot_b_L2);	      	
   								pad_press = 1;
   								//printf("L2 pressed\n"); 
  							}
							//goto test_boot_files;	      	   	  				
  						}	
						if ((Timer() > (WaitTime + 3000)) || (pad_press)) goto test_boot_files; // wait pad button during 3 sec.
  						
  					} 
  		
test_boot_files:	       

  					execPathPoint = mass_boot;
  					if ((fdn = open(mass_boot, O_RDONLY)) <= 0)
  					{
    					execPathPoint = mc_boot;
    					if ((fdn = open(mc_boot, O_RDONLY)) <= 0)
    					{
      						mc_boot[2]++;
      						execPathPoint = mc_boot;
      						if ((fdn = open(mc_boot, O_RDONLY)) <= 0)
      						{
        						execPathPoint = mc_boot_b;
        						if ((fdn = open(mc_boot_b, O_RDONLY)) <= 0)
        						{
          							mc_boot_b[2]++;
          							execPathPoint = mc_boot_b;
          							if ((fdn = open(mc_boot_b, O_RDONLY)) <= 0)
          							{
	          							//launch_browser();
            							SETBG(0xff, 0, 0); // File not found, Red screen
            							while (1) {;}
          							}
        						}
      						}
    					}
  					}
  					close(fdn);
	   	   	  
  				}	  
			}
	  }
  }
  close(fdn);

    
  exd.epc = 0;
  exd.gp = 0;
  exd.sp = 0;
  
  r = SifLoadElf(execPathPoint, &exd);
  if ((!r) && (exd.epc))
  {
    fioExit();
    SifExitRpc(); //some programs need it to be here

    FlushCache(0);
    FlushCache(2);
    argv[0] = execPathPoint;

    ExecPS2((void*)exd.epc, (void*)exd.gp, 1, argv);

    SETBG(0x00, 0x00, 0xff); // Error, Blue screen
    while (1){;}
  }

  SETBG(0xff, 0xff, 0xff);  // Error2, White screen
  while (1){;}
}
