/*      
  _____     ___ ____ 
   ____|   |    ____|      PSX2 OpenSource Project
  |     ___|   |____       (c) 2008 jMk0Nm 
------------------------------------------------------------------------
mcid.c

Handles file decryption/encryption 

Uses mcsio2 module that must be loaded and inited before with
int mcsio2_rpc_Init(void)
XSIO2MAN must be loaded too.

main funcs :
void *Decrypt_Card_File(void *buf) : decrypt an encrypted file for memory card.
void *Decrypt_Disk_File(void *buf) : decrypt an encrypted file for disk or rom.
void *Encrypt_Disk_File(void *buf) : Encrypt an encrypted (that's the sony trick) file from disk or rom to memory card.

*/
 
#include "mcboot.h" 
 
 /* sio2 packet structs */  
 
struct _sio2_dma_arg { // size 12
	void	*addr;
	int	size;
	int	count; 
};

typedef struct {
	u32	stat6c;
	u32	port_ctrl1[4];
	u32	port_ctrl2[4]; 
	u32	stat70;
	u32	regdata[16]; 
	u32	stat74; 
	u32	in_size; 
	u32	out_size; 
	u8	*in;
	u8	*out; 
	struct _sio2_dma_arg in_dma; 
	struct _sio2_dma_arg out_dma;
} Sio2Packet;
 
 
 typedef struct {
	u32    ret; 
 	u32    stat6c;
 	u32    port_ctrl1[4];
 	u32    port_ctrl2[4];
 	u32    stat70;   
 	u32    regdata[16];
 	u32    stat74;   
 	u32    in_size; 
 	u32    out_size; 
 	u8     in[14]; 
 	u8     out[14]; 
 } Rpc_Packet_Send_SIO2;

typedef struct {
	int ret;
    u16 port;
    u16 slot;
    u16 a2;    
} Rpc_Packet_Send_CARDAUTH;	


int mcman_getcnum (int port, int slot);

 
#define MCSIO2_IRX 0x0E0E0E0

static SifRpcClientData_t mcsio2    __attribute__((aligned(64)));
static int Rpc_Buffer[1024] __attribute__((aligned(64)));

int MCSIO2_Inited  = 0;

//--------------------------------------------------------------
int mcsio2BindRpc(void) {
	int ret;
	int retryCount = 0x1000;

	while(retryCount--) {
	        ret = SifBindRpc( &mcsio2, MCSIO2_IRX, 0);
        	if ( ret  < 0)  {
			  	printf("mcsio2: EE Bind RPC Error.\n");
	          	return -1;
	        }
	        if (mcsio2.server != 0){
	        	printf("mcsio2: EE Bind RPC Set.\n");
	        	break;
	        }

	        // short delay 
	      	ret = 0x10000;
	    	while(ret--) asm("nop\nnop\nnop\nnop");
	}

	MCSIO2_Inited = 1;
	return retryCount;
}
//--------------------------------------------------------------
int mcsio2_rpc_Init(void)
{
 	mcsio2BindRpc();

 	if(!MCSIO2_Inited)
 	{
 		scr_printf("\tmcsio2 RPC Init failed\n");
 		return -1;
	}
	
 	return 1;
}
//--------------------------------------------------------------
int mcsio2_transfer(Sio2Packet *sio2data)
{
	int i;
	Rpc_Packet_Send_SIO2 *Packet = (Rpc_Packet_Send_SIO2 *)Rpc_Buffer;

 	if(!MCSIO2_Inited) 
 	{
		printf("\tmcsio2 not Inited\n");	 	
	 	return -1;
 	}
		
	Packet->stat6c = sio2data->stat6c;
	for(i = 0; i < 4; i++)
	{
 		Packet->port_ctrl1[i] = sio2data->port_ctrl1[i];
 		Packet->port_ctrl2[i] = sio2data->port_ctrl2[i];
	}
 	Packet->stat70 = sio2data->stat70;   
	for(i = 0; i < 16; i++) Packet->regdata[i] = sio2data->regdata[i];
 	Packet->stat74 = sio2data->stat74;   
 	Packet->in_size = sio2data->in_size; 
 	Packet->out_size = sio2data->out_size; 
 	memcpy(Packet->in, sio2data->in, sio2data->in_size);
 	memcpy(Packet->out, sio2data->out, sio2data->out_size); 	
 	
 
   	if ((SifCallRpc(&mcsio2, 1, 0, (void*)Rpc_Buffer, sizeof(Rpc_Packet_Send_SIO2), (void*)Rpc_Buffer, sizeof(Rpc_Packet_Send_SIO2),0,0)) < 0) 
   	{
		printf("\tmcsio2_transfer: RPC error\n");
	    return -1;
    }

	sio2data->stat6c = Packet->stat6c;
	for(i = 0; i < 4; i++)
	{
 		sio2data->port_ctrl1[i] = Packet->port_ctrl1[i];
 		sio2data->port_ctrl2[i] = Packet->port_ctrl2[i];
	}
 	sio2data->stat70 = Packet->stat70;   
	for(i = 0; i < 16; i++) sio2data->regdata[i] = Packet->regdata[i];
 	sio2data->stat74 = Packet->stat74;   
 	sio2data->in_size = Packet->in_size; 
 	sio2data->out_size = Packet->out_size; 
 	memcpy(sio2data->in, Packet->in, Packet->in_size);
 	memcpy(sio2data->out, Packet->out, Packet->out_size); 	
 	
	
 	return Packet->ret;
}

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

int Card_Auth(void)
{
 	Rpc_Packet_Send_CARDAUTH *Packet = (Rpc_Packet_Send_CARDAUTH *)Rpc_Buffer;

 	if(!MCSIO2_Inited) 
 	{
		printf("\tmcsio2 not Inited\n");	 	
	 	return -1;
 	}
 	
  	Packet->port = 2;
 	Packet->slot = 0; 
	Packet->a2 = mcman_getcnum (Packet->port, Packet->slot);  	
 
   	if ((SifCallRpc(&mcsio2, 2, 0, (void*)Rpc_Buffer, sizeof(Rpc_Packet_Send_CARDAUTH), (void*)Rpc_Buffer, sizeof(int),0,0)) < 0) 
   	{
		printf("\tCard_Auth: RPC error\n");
	    return -1;
    }

 	return Packet->ret;
}

//_______________________________________________________________________________

 int mcman_getcnum (int port, int slot)
 {
	 return (port & 1) << 3;
 }
  
 //---------------------------------------------------------------------------------------------------------------


int mcsio2_buf_checksum(u8 *buf, int size)
 {
	 int i;
	 int checksum = 0;
	 
	 if (size <= 0) return checksum & 0xFF; 
  	 for (i = 0; i < size; i++) checksum ^= buf[i];
	 return checksum & 0xFF; 
 }

//_______________________________________________________________________________
 
int mcsio2_MagicGate_Cmd_2(int port, int slot, u8 *buf, int a3, int cmd)
 {
	 int i;
	 u8 sio2buf[148];
	 u8 wrbuf[14];
	 u8 rdbuf[14];
	 
     Sio2Packet *sio2packet = (Sio2Packet *) sio2buf;	
	 sio2packet->in = (u8 *)&wrbuf;
	 sio2packet->out = (u8 *)&rdbuf;	 	 
 	 for (i=0; i < 4; i++)
 	 {
	 	 sio2packet->port_ctrl1[i] = 0;
	 	 sio2packet->port_ctrl2[i] = 0;	 	 
 	 }
 	 sio2packet->port_ctrl1[port] = 0xFF060505;
	 sio2packet->port_ctrl2[port] = 0x0003FFFF;
 	 sio2packet->regdata[0] = (port & 3) | 0x00380E40;
 	 sio2packet->regdata[1] = 0;
	 sio2packet->out_size = 14;
	 sio2packet->in_size = 14;
	 memset(wrbuf, 0, 14);
	 wrbuf[0] = 0x81;
	 wrbuf[1] = (u8)a3;
	 wrbuf[2] = (u8)cmd;
  	 for (i = 0; i < 8; i++) wrbuf[3 + i] = buf[7 - i];
	 wrbuf[11] = (u8)mcsio2_buf_checksum(buf, 8);
	 if (!(mcsio2_transfer((Sio2Packet *)sio2packet)))
	 {
		 printf("mcsio2 transfer error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (((sio2packet->stat6c >> 13) & 1) || ((sio2packet->stat6c >> 14) & 3))
	 {
		 printf("sio2 command error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (rdbuf[12] != 0x2B) 
	 {
		 printf("ID error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (rdbuf[13] == 0x66) 
	 {
		 printf("result failed %d\n", cmd & 0xFF);
		 return 0;
	 }
	 return 1;
 }
 
//---------------------------------------------------------------------------------------------------------------
 
int mcsio2_MagicGate_Cmd_1(int port, int slot, u8 *buf, int a3, int cmd)
 {
	 int i;
	 u8 sio2buf[148];
	 u8 wrbuf[14];
	 u8 rdbuf[14];
	 	 
     Sio2Packet *sio2packet = (Sio2Packet *) sio2buf;	
	 sio2packet->in = (u8 *)&wrbuf;
	 sio2packet->out = (u8 *)&rdbuf;	 	 
 	 for (i = 0; i < 4; i++)
 	 {
	 	 sio2packet->port_ctrl1[i] = 0;
	 	 sio2packet->port_ctrl2[i] = 0;	 	 
 	 }
 	 sio2packet->port_ctrl1[port] = 0xFF060505;
	 sio2packet->port_ctrl2[port] = 0x0003FFFF;
 	 sio2packet->regdata[0] = (port & 3) | 0x00380E40;
 	 sio2packet->regdata[1] = 0;
	 sio2packet->out_size = 14;
	 sio2packet->in_size = 14;
	 memset(wrbuf, 0, 14);
	 wrbuf[0] = 0x81;
	 wrbuf[1] = (u8)a3;
	 wrbuf[2] = (u8)cmd;
	 if (!(mcsio2_transfer((Sio2Packet *)sio2packet)))
	 {
		 printf("mcsio2 transfer error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (((sio2packet->stat6c >> 13) & 1) || ((sio2packet->stat6c >> 14) & 3))
	 {
		 printf("sio2 command error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (rdbuf[3] != 0x2B) 
	 {
		 printf("ID error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (rdbuf[13] == 0x66) 
	 {
		 printf("result failed %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (((u8)mcsio2_buf_checksum(rdbuf + 4, 8) & 0xFF) != rdbuf[12]) 
	 {
		 printf("mcsio2 checksum error %d\n", cmd & 0xFF);
		 return 0;
	 }
  	 for (i = 0; i < 8; i++) buf[7 - i] = rdbuf[4 + i];
	 return 1; 
 }

 
//---------------------------------------------------------------------------------------------------------------
 
int mcsio2_MagicGate_Cmd_0(int port, int slot, int a2, int cmd)
 {
	 int i;
	 u8 sio2buf[148];
	 u8 wrbuf[5];
	 u8 rdbuf[5];
	 	 
     Sio2Packet *sio2packet = (Sio2Packet *) sio2buf;	
	 sio2packet->in = (u8 *)&wrbuf;
	 sio2packet->out = (u8 *)&rdbuf;	 	 
 	 for (i=0; i < 4; i++)
 	 {
	 	 sio2packet->port_ctrl1[i] = 0;
	 	 sio2packet->port_ctrl2[i] = 0;	 	 
 	 }
 	 sio2packet->port_ctrl1[port] = 0xFF060505;
	 sio2packet->port_ctrl2[port] = 0x0003FFFF;
 	 sio2packet->regdata[0] = (port & 3) | 0x00140540;
 	 sio2packet->regdata[1] = 0;
	 sio2packet->out_size = 5;
	 sio2packet->in_size = 5;
	 memset(wrbuf, 0, 5);
	 wrbuf[0] = 0x81;
	 wrbuf[1] = (u8)a2;
	 wrbuf[2] = (u8)cmd;
	 if (!(mcsio2_transfer((Sio2Packet *)sio2packet)))
	 {
		 scr_printf("mcsio2 transfer error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (((sio2packet->stat6c >> 13) & 1) || ((sio2packet->stat6c >> 14) & 3))
	 {
		 scr_printf("sio2 command error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (rdbuf[3] != 0x2B) 
	 {
		 scr_printf("ID error %d\n", cmd & 0xFF);
		 return 0;
	 }
	 if (rdbuf[4] == 0x66) 
	 {
		 scr_printf("result failed %d\n", cmd & 0xFF);
		 return 0;
	 }
	 return 1; 
 }
//_______________________________________________________________________________

 int mecha_command(int cmd, u8 *argp, int arglen, u8 *buf)
 {
	  return cdApplySCmd(cmd, argp, arglen, buf, 16);	
 }
 
//---------------------------------------------------------------------------------------------------------------  

 int Mecha_write_data(void *data, int size)
 {
	 u8 ret[1];
	 u8 mecha_param[16];	 
	 
	 if (size > 16) return 0;
	 if (size > 0) memcpy(mecha_param, data, size);
	 if (!(mecha_command(0x8D, mecha_param, size, ret))) return 0;
	 if (!((int)ret[0])) return 1;
	 return 0;
 }
   
 //---------------------------------------------------------------------------------------------------------------
 
 int Mecha_read_data(void *data, int size)
 {
	 u8 ret[16];
	 
	 if (size > 16) return 0;
	 if (!(mecha_command(0x8E, NULL, 0, ret))) return 0;
	 if (size <= 0) return 1;
	 memcpy(data, ret, size);
	 return 1;
 }
 
 //---------------------------------------------------------------------------------------------------------------
 
 int Mecha_pol_cal_cmplt(void)
 {
	 u8 ret[1];
	 
	 do if (!(mecha_command(0x8F, NULL, 0, ret))) return 0;
	 while ((int)ret[0]);
	 if (!((int)ret[0])) return 1;
	 return 0;
 }
 
 //--------------------------------------------------------------------------------------------------------------- 
 
 int  Mecha_ReadKbit1(void* kbit_part1)
 {
	 u8 ret[9];	 
	 
	 if (!(mecha_command(0x94, NULL, 0, ret))) return 0;
 	 if ((int)ret[0]!= 0) return 0;
 	 memcpy(kbit_part1, ret + 1, 8);
	 return 1; 	 
 }

 //--------------------------------------------------------------------------------------------------------------- 
 
 int  Mecha_ReadKbit2(void* kbit_part2)
 {
	 u8 ret[9];	 
	 
	 if (!(mecha_command(0x95, NULL, 0, ret))) return 0;
 	 if ((int)ret[0]!= 0) return 0;
 	 memcpy(kbit_part2, ret + 1, 8);
	 return 1; 	 
 }

 //--------------------------------------------------------------------------------------------------------------- 
 
 int  Mecha_ReadKc1(void* kc_part1)
 {
	 u8 ret[9];	 
	 
	 if (!(mecha_command(0x96, NULL, 0, ret))) return 0;
 	 if (((int)ret[0])!= 0) return 0;
 	 memcpy(kc_part1, ret + 1, 8);
	 return 1; 	 
 }
 //--------------------------------------------------------------------------------------------------------------- 
 
 int  Mecha_ReadKc2(void* kc_part2)
 {
	 u8 ret[9];	 
	 
	 if (!(mecha_command(0x97, NULL, 0, ret))) return 0;
 	 if (((int)ret[0])!= 0) return 0;
 	 memcpy(kc_part2, ret + 1, 8);
	 return 1; 	 
 }
 
 //--------------------------------------------------------------------------------------------------------------- 
 
 int  Mecha_ReadICVPS2(void* icvps2)
 {
	 u8 ret[9];	 
	 
	 if (!(mecha_command(0x98, NULL, 0, ret))) return 0;
 	 if (((int)ret[0])!= 0) return 0;
 	 memcpy(icvps2, ret + 1, 8);
	 return 1; 	 
 }

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

 int Mecha_write_HD_start(int hcode, int cnum, int a2, int hlength)
 {
	 u8 ret[1];	 
	 u8 param_buf[5];
   	 
	 param_buf[0] = (u8)hcode;
	 param_buf[1] = (u8)hlength;
	 param_buf[2] = (u8)(hlength>>8);
	 param_buf[3] = (u8)cnum;	 	 	 
	 param_buf[4] = (u8)a2;
	 if (!(mecha_command(0x90, param_buf, 5, ret))) return 0;
	 if (!((int)ret[0])) return 1;
	 return 0;
 }
 
 //---------------------------------------------------------------------------------------------------------------

 int Mecha_ReadBITLength(int *bit_size)
 {
	 u8 ret[3];
	  
	 if (!(mecha_command(0x91, NULL, 0, ret))) return 0;
	 if (((int)ret[0])!= 0) return 0;
	 *bit_size = (int)ret[1] + (int)(ret[2]<<8);
	 return 1;
 }
 
 //---------------------------------------------------------------------------------------------------------------

 int Mecha_WriteDataInLength(int size)
 {
	 u8 ret[1];	 
	 u8 param_buf[2];
	 
 	 param_buf[0] = (u8)size;
	 param_buf[1] = (u8)(size>>8);
	 if (!(mecha_command(0x92, param_buf, 2, ret))) return 0;
	 if (!((int)ret[0])) return 1;
	 return 0;
 }

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

 int Mecha_WriteDataOutLength(int size)
 {
	 u8 ret[1];	 
	 u8 param_buf[2];
	 
 	 param_buf[0] = (u8)size;
	 param_buf[1] = (u8)(size>>8);
	 if (!(mecha_command(0x93, param_buf, 2, ret))) return 0;
	 if (!((int)ret[0])) return 1;
	 return 0;	 
 }
  
//---------------------------------------------------------------------------------------------------------------

 int Mecha_ReadBIT(void* bit)
 {
	int offset;
	int bit_length, bitlength;
	int *p_bitlength = &bit_length;
	
	if (!(Mecha_ReadBITLength(p_bitlength))) return 0; // send mecha command 0x91
	bitlength = bit_length;
	if ((bit_length) > 0) 
	{
 		offset = 0;
 		do 
 		{	 	 
 	 		if ((bit_length & 0xFFFF) < 16)
 	 		{
	 			if (!(Mecha_read_data(bit + offset, bit_length & 0xFFFF))) return 0;  // send mecha command 0x8E
	 			bit_length = 0;		 	 
 	 		}
 	 		else 
 	 		{
	 	 		if (!(Mecha_read_data(bit + offset, 16))) return 0; // send mecha command 0x8E
	 	 		offset += 16;
	 	 		bit_length -= 16;
 	 		}
 		} while ((bit_length & 0xFFFF) > 0);
	}
	return bitlength;
 } 
 
 //---------------------------------------------------------------------------------------------------------------
 
 int Mecha_ReadBLOCK(void* buf, int size)
 {
	int offset;
	 
	if (!(Mecha_WriteDataOutLength(size & 0xFFFF))) return 0; // send mecha command 0x93
	if (size <= 0) return 1;
 	offset = 0;
 	do 
 	{	 	 
 	 	if (size < 16)
 	 	{
	 		if (!(Mecha_read_data(buf + offset, size))) return 0;  // send mecha command 0x8E
	 		size = 0;		 	 
 	 	}
 	 	else 
 	 	{
	 	 	if (!(Mecha_read_data(buf + offset, 16))) return 0; // send mecha command 0x8E
	 	 	offset += 16;
	 	 	size -= 16;
 	 	}
 	} while (size > 0);
	return 1;
 } 
 
//---------------------------------------------------------------------------------------------------------------

 int Mecha_Set_Header(int hcode, int cnum, int a2, void* buf)
 {
	 int hlength, offset;
	 int r;
	 
	 hlength = ((mg_ELF_Header_t *) buf)->mg_header_size;
	 printf("header length %d\n", hlength & 0xFFFF);
	 if (!Mecha_write_HD_start(hcode & 0x00FF, cnum, a2, hlength & 0xFFFF))  // send mecha command 0x90
	 {
		 printf("Mecha_set_header: fail Mecha_write_HD_start\n");
		 return 0;
 	 }
 	 if ((hlength & 0xFFFF) > 0) // assume 0 < hlength < 65536
 	 {
	 	offset = 0;
	 	do 
	 	{	 	 
	 	 	if ((hlength & 0xFFFF) < 16)
	 	 	{
		 		r = Mecha_write_data(buf + offset, hlength & 0xFFFF);  // send mecha command 0x8D
		 		hlength = 0;		 	 
	 	 	}
	 	 	else 
	 	 	{
		 	 	r = Mecha_write_data(buf + offset, 16); // send mecha command 0x8D
		 	 	offset += 16;
		 	 	hlength -= 16;

	 	 	}
	 	 	if (r == 0) 
	 	 	{
		 		printf("Mecha_set_header: fail write_data\n");
		 		return 0;
	 	 	}
 	 	} while ((hlength & 0xFFFF) > 0);
 	 }
	 if (!(Mecha_pol_cal_cmplt()))  // send mecha command 0x8F
	 {
		printf("Mecha_set_header: fail pol_cal_cmplt\n");
		return 0;
	 }
	 return 1;
	 
 } 

//---------------------------------------------------------------------------------------------------------------
 
 int Mecha_Set_Block(void *buf, int size) 
 {
	 int offset, r;
	 
	 if (!(Mecha_WriteDataInLength(size & 0xFFFF))) return 0; // send mecha command 0x92
 	 if (size > 0) 
 	 {
	 	offset = 0;
	 	do 
	 	{	 	 
	  		if (size < 16)
	  		{
	 			r = Mecha_write_data(buf + offset, size);  // send mecha command 0x8D
	 			size = 0;		 	 
 	 		}
 	 		else 
 	 		{
	 	 		r = Mecha_write_data(buf + offset, 16); // send mecha command 0x8D
	 	 		offset += 16;
	 	 		size -= 16;
 	 		}
 	 		if (r == 0) return 0;
 	 	} while (size > 0);
	 }
	 if (!(Mecha_pol_cal_cmplt())) return 0; // send mecha command 0x8F
	 return 1;
	 	  
 }
 
//---------------------------------------------------------------------------------------------------------------

 int GetKbit(void *kbit) 
 {
	if (!(Mecha_ReadKbit1(kbit))) return 0;
	if (!(Mecha_ReadKbit2(kbit + 8))) return 0;
    return 1;	
 }

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

 int GetKc(void *kc) 
 {
	if (!(Mecha_ReadKc1(kc))) return 0;
	if (!(Mecha_ReadKc2(kc + 8))) return 0;
    return 1;	
 }
 
//---------------------------------------------------------------------------------------------------------------

 int GetICVPS2(void *icvps2) 
 {
	if (!(Mecha_ReadICVPS2(icvps2))) return 0;
    return 1;	
 }
 
//---------------------------------------------------------------------------------------------------------------
 
 int Card_Encrypt(int port, int slot, void *buf) 
 {
	if (!(mcsio2_MagicGate_Cmd_0(port, slot, 0xF2, 0x50))) return 0;
	if (!(mcsio2_MagicGate_Cmd_2(port, slot, buf, 0xF2, 0x51))) return 0;	
	if (!(mcsio2_MagicGate_Cmd_0(port, slot, 0xF2, 0x52))) return 0;
	if (!(mcsio2_MagicGate_Cmd_1(port, slot, buf, 0xF2, 0x53))) return 0;	
	return 1;
 }
  
//---------------------------------------------------------------------------------------------------------------

 int Card_Decrypt(int port, int slot, void *buf) 
 {
	if (!(mcsio2_MagicGate_Cmd_0(port, slot, 0xF1, 0x40))) return 0;
	if (!(mcsio2_MagicGate_Cmd_2(port, slot, buf, 0xF1, 0x41))) return 0;	
	if (!(mcsio2_MagicGate_Cmd_0(port, slot, 0xF1, 0x42))) return 0;
	if (!(mcsio2_MagicGate_Cmd_1(port, slot, buf, 0xF1, 0x43))) return 0;	
	return 1;
 }
 
 //---------------------------------------------------------------------------------------------------------------
 
void store_kbit(u8 *inbuf, u8 *kbit)
{	
	/* Stores bit Key at right offset in a encrypted Header */	
  	mg_ELF_Header_t *header = (mg_ELF_Header_t *) inbuf;
  	int offset = 0x20;
  	
  	if (header->BIT_count > 0) offset += header->BIT_count * 16;
  	if ((*(u32*)&header->flags) & 1) offset += inbuf[offset] + 1;
  	if (((*(u32*)&header->flags) & 0xf000) == 0) offset += 8;
 	memcpy(inbuf + offset, kbit, 16);
  	return; 
}

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

void store_kc(u8 *inbuf, u8 *kc)
{	
	/* Stores content Key at right offset in a encrypted Header */			
  	mg_ELF_Header_t *header = (mg_ELF_Header_t *) inbuf;
  	int offset = 0x20;

  	if (header->BIT_count > 0) offset += header->BIT_count * 16;
  	if ((*(u32*)&header->flags) & 1) offset += inbuf[offset] + 1;
  	if (((*(u32*)&header->flags) & 0xf000) == 0) offset += 8;
 	offset += 16;
	memcpy(inbuf + offset, kc, 16);
  	return; 
}

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

void store_icvps2(u8 *inbuf, u8 *icvps2)
{
	/* Stores ICVPS2 at right offset in a encrypted Header */		
 	mg_ELF_Header_t *header = (mg_ELF_Header_t *) inbuf;
 	int offset = header->mg_header_size - 8;

 	memcpy(inbuf + offset, icvps2, 8);
 	return;   
  
}

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

void read_kbit(u8 *inbuf, u8 *kbit){

  	int offset = 0x20;
  	mg_ELF_Header_t *header = (mg_ELF_Header_t *) inbuf;
  	
  	if (header->BIT_count > 0) offset += header->BIT_count * 16;
  	if ((*(u32*)&header->flags) & 1) offset += inbuf[offset] + 1;
  	if (((*(u32*)&header->flags) & 0xf000) == 0) offset += 8;
	memcpy (kbit, inbuf + offset, 16);
 	return;      
}

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

void read_kc(u8 *inbuf, u8 *kc){

	int offset = 0x20; 
  	mg_ELF_Header_t *header = (mg_ELF_Header_t *) inbuf;
  	
  	if (header->BIT_count > 0) offset += header->BIT_count * 16;
  	if ((*(u32*)&header->flags) & 1) offset += inbuf[offset] + 1;
  	if (((*(u32*)&header->flags) & 0xf000) == 0) offset += 8;
	offset += 16;
	memcpy (kc, inbuf + offset, 16);
	return;
}

//---------------------------------------------------------------------------------------------------------------
 
 int Decrypt_Disk_Header (void *buf, void *bit, int *psize)
 {
    int size;
 	psize= &size; 	    
 	int hcode = 0; // 0 = DiskBoot 1 = CardBoot 2 = Download 
 	int cnum = 0; 
 	
 	if (!Mecha_Set_Header(hcode, cnum, 0, buf)) return 0; // send mecha commands 0x90, 0x8D, 0x8F
 	size = Mecha_ReadBIT(bit);
 	if (!size) return 0; // send mecha commands 0x91, 0x8E	
 	return 1; 	
 }  
 
//---------------------------------------------------------------------------------------------------------------
 
 int Download_Header (int port, int slot, void *buf, void *bit, int *psize)
 {
	int size;
	psize = &size;
 	int hcode = 2; // 0 = DiskBoot 1 = CardBoot 2 = Download 
 	
 	int cnum = mcman_getcnum(port, slot); 
 	if (cnum < 0) return 0;
	if (!(Mecha_Set_Header(hcode, cnum, 0, buf))) return 0; // send mecha commands 0x90, 0x8D, 0x8F
 	size = Mecha_ReadBIT(bit);
 	if (!size) return 0; // send mecha commands 0x91, 0x8E	
 	return 1; 	
 } 
 
//---------------------------------------------------------------------------------------------------------------
 
 int Decrypt_Card_Header (int port, int slot, void *buf, void *bit, int *psize)
 {
 	u8 kbit[16];
 	u8 kc[16]; 	
 	int size; 
 	psize = &size; 	
 	  	
 	read_kbit(buf, kbit);
 	read_kc(buf, kc); 	
 	
 	if (!Card_Decrypt(port, slot, kbit)) return 0;
 	if (!Card_Decrypt(port, slot, kbit + 8)) return 0;
 	if (!Card_Decrypt(port, slot, kc)) return 0;
 	if (!Card_Decrypt(port, slot, kc + 8)) return 0;
 	
 	store_kbit(buf, kbit);
 	store_kc(buf, kc); 	
 	
 	int hcode = 1; // 0 = DiskBoot 1 = CardBoot 2 = Download 
 	int cnum = mcman_getcnum(port, slot); 
 	if (cnum < 0) return 0;
	
 	if (!Mecha_Set_Header(hcode, cnum, 0, buf)) return 0; // send mecha commands 0x90, 0x8D, 0x8F
 	size = Mecha_ReadBIT(bit);
 	if (!size) return 0; // send mecha commands 0x91, 0x8E	
	    
 	return 1; 	
 }
 
//---------------------------------------------------------------------------------------------------------------
 int Decrypt_Block (void *src, void *dst, int size) 
 {
 	if (!Mecha_Set_Block(src, size)) return 0; // send mecha commands 0x92, 0x8D, 0x8F	 
 	if (!(Mecha_ReadBLOCK(dst, size))) return 0; // send mecha commands 0x93, 0x8E 	
 	return 1;
 }
//---------------------------------------------------------------------------------------------------------------
 int Download_Block (void *src, int size) 
 {
 	if (!Mecha_Set_Block(src, size)) return 0; // send mecha commands 0x92, 0x8D, 0x8F	 
 	return 1;
 }
//---------------------------------------------------------------------------------------------------------------

int bit_offset(u8 *inbuf)
{	/* returns the bit table offset */
	
  	mg_ELF_Header_t *header = (mg_ELF_Header_t *) inbuf;
  	int offset = 0x20;

  	if (header->BIT_count > 0) offset += header->BIT_count * 16;
  	if ((*(u32*)&header->flags) & 1) offset += inbuf[offset] + 1;
  	if (((*(u32*)&header->flags) & 0xf000) == 0) offset += 8;
 	return offset + 0x20; 
}

//---------------------------------------------------------------------------------------------------------------
 
 int Download_GetKbit (int port, int slot, void *kbit)
 { 
	if (!GetKbit(kbit)) return 0; // send mecha commands 0x94, 0x95
	if (!Card_Encrypt(port, slot, kbit)) return 0;
	if (!Card_Encrypt(port, slot, kbit + 8)) return 0;
 	return 1;
 }

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

 int Download_GetKc (int port, int slot, void *kc)
 { 
	if (!GetKc(kc)) return 0;  // send mecha commands 0x96, 0x97
  	if (!Card_Encrypt(port, slot, kc)) return 0;
 	if (!Card_Encrypt(port, slot, kc + 8)) return 0;
 	return 1;
 }
 
//------------------------------------------------------------------------------------------------------------------------ 

 int Download_GetICVPS2 (void *icvps2)
 { 
	if (!GetICVPS2(icvps2)) return 0;  // send mecha command 0x98
 	return 1;
 }

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

void *Encrypt_Disk_File(void *buf)
{
  	u8 bit[0x400];
  	u8 kbit[16];
  	u8 kcontent[16];
  	u8 icvps2[8];
  	int size, bit_ofs;
  	int *psize = &size;

  	int i;

  	printf("\tEncrypt_Disk_File ");
  	  	
	bit_ofs = bit_offset(buf);
	
    Bit_Data_t *b = (Bit_Data_t *) bit;  	
	memcpy (bit, buf + bit_ofs, 1024);

  	if (!Download_Header(2, 0, buf, bit, psize)) {
    	printf("Failed to decrypt header\n");
    	return NULL;
  	}
  	
  	if (b->block_count > 0) {
    	int offset = b->headersize;
    	for (i = 0; i < b->block_count; i++) {
      		if (b->blocks[i].flags & 2) {
        		if (!Download_Block(buf + offset, b->blocks[i].size)) {
          			printf("Failed\n");
          			return NULL;
        		}
      		}
      		offset += b->blocks[i].size;
    	}
  	}

  	if (!Download_GetKbit(2, 0, kbit)) {
    	printf("Failed to get kbit\n");
    	return NULL;
  	}
  	
  	if (!Download_GetKc(2, 0, kcontent)) {
    	printf("Failed to get kc\n");
    	return NULL;
  	}
  	
  	store_kbit(buf, kbit);
  	store_kc(buf, kcontent);

  	if (((mg_ELF_Header_t *) buf)->flags & 2) {
    	if (!Download_GetICVPS2(icvps2)) {
      		printf("Failed to get icvps2\n");
      		return NULL;
    	}
    	store_icvps2(buf, icvps2);
  	}
  	
  	printf("Done\n");
  	
  	return (void *) buf;
}
//------------------------------------------------------------------------------------------------------------------------ 

void *Decrypt_Disk_File(void *buf)
{
	
	int i, r, bit_ofs;
	u8 bit[0x400];
  	int size;
  	int *psize = &size;
	
  	printf("\n\tDecrypting file ");        
  	
	bit_ofs = bit_offset(buf);
	
	r = Decrypt_Disk_Header(buf, buf + bit_ofs, psize);
	if (r == 0) {
		printf ("Failed to decrypt header\n");
		return NULL;
    }
    Bit_Data_t *b = (Bit_Data_t *) bit;	
	memcpy (bit, buf + bit_ofs, 1024);
	        
	if (b->block_count > 0) {
    	int offset = b->headersize;
    	for (i = 0; i < b->block_count; i++) {
	    	if (b->blocks[i].flags & 3) {
				r = Decrypt_Block(buf + offset, buf + offset, b->blocks[i].size);
				if (r == 0) {
					printf ("Failed\n");
					return NULL;
				}	
	    	}
	    	offset += b->blocks[i].size;	
    	}

	}
	
  	printf("Done\n");	

	return (void*) (buf + (((mg_ELF_Header_t *) buf)->mg_header_size & 0xFFFF));	

}

//_______________________________________________________________________________

void *Decrypt_Card_File(void *buf)
{
	
	int r, bit_ofs, i;
	u8 bit[0x400];
	int size;
	int *psize = &size;
	
	printf("\n\tDecrypting file ");
        
	bit_ofs = bit_offset(buf);
	
	r = Decrypt_Card_Header(2, 0, buf, buf + bit_ofs, psize);
	if (r==0) {
		printf ("Failed to decrypt header\n");
		return NULL;
    }
    
    Bit_Data_t *b = (Bit_Data_t *) bit;	    
	memcpy (bit, buf + bit_ofs, sizeof(bit));    
	
	if (b->block_count > 0) {
    	int offset = b->headersize;
    	for (i = 0; i < b->block_count; i++) {
	    	if (b->blocks[i].flags & 3) {
				r = Decrypt_Block(buf + offset, buf + offset, b->blocks[i].size);
				if (r==0) {
					printf ("Failed\n");
					return NULL;
				}	
	    	}
	    	offset += b->blocks[i].size;	
    	}

	}	

  	printf("Done\n");	
  		
	return (void*) (buf + (((mg_ELF_Header_t *) buf)->mg_header_size & 0xFFFF));	
}

//_______________________________________________________________________________
