//
// Nokia Security Master Code
// by indear
//
// Extract from Mcode app by Dejan Kaljevic
//

#include <stdio.h>
#include <stdlib.h>

unsigned int unk;

unsigned char tabla1[] = {
	0xB1, 0xB1, 0x73, 0x73, 0xE6, 0xE6,
    0x5A, 0x5A, 0xAB, 0xAB, 0x47, 0x47,
    0x8E, 0x8E, 0x0D, 0x0D, 0x1A, 0x1A,
	0x34, 0x34, 0x68, 0x68, 0x0B, 0x0B
};

unsigned char tabla2[] = {
	0x34, 0x62, 0x58, 0x92, 0x76, 0x13,
    0x25, 0x97, 0x56, 0x28, 0x68, 0x13
};

unsigned short gethalf(unsigned char *donde)
{
	return ((donde[1]<<8)|donde[0]);
}

void sethalf(unsigned char *donde, unsigned short que)
{
	donde[0] = que & 0xFF;
	donde[1] = (que>>8) & 0xFF;
}

void inv(unsigned char *cadena)
{
	int i;
	unsigned char temp;

	for(i=0; i<=10; i+=2)
	{
		temp = cadena[i];
		cadena[i] = cadena[i+1];
		cadena[i+1] = temp;
	}
}

unsigned int calculo4a(unsigned int param0)
{
	unsigned int r0, r1, r2;
	int i;
	
	r1 = unk;
	r0 = param0 << 0x10;
	r2 = r0 >> 0x10;
	
	for(i=0; i<=0xf; i++)
	{
        r1 = r1 << 1;
        r0 = r2;
        r0 &= 1;
        r1 |= r0;
        r2 = r2 >> 1;
    }
    
    r0 = r1;
    
    return r0;
}

void calculo4(unsigned char *param0)
{
	unsigned int r0, r4, r6;
	
	r0 = gethalf(param0+8);
	r0 = calculo4a(r0);
	r6 = r0;
	r6 = r6 << 0x10;
	r6 = r6 >> 0x10;

	r0 = gethalf(param0+0xa);
	r0 = calculo4a(r0);
	r4 = r0;
	r4 = r4 << 0x10;
	r4 = r4 >> 0x10;

	r0 = gethalf(param0+2);
	r0 = calculo4a(r0);
	sethalf(param0+8, r0);

	r0 = gethalf(param0);
	sethalf(param0+0xa, r0);
	sethalf(param0, r4);
	sethalf(param0+2, r6);
	
	r0 = gethalf(param0+4);
	r0 = calculo4a(r0);
	r4 = r0;
	r4 = r4 << 0x10;
	r4 = r4 >> 0x10;
	
	r0 = gethalf(param0+6);
	r0 = calculo4a(r0);
	sethalf(param0+4, r0);
	sethalf(param0+6, r4);
}

void calculo3(unsigned char *param0)
{
	unsigned int r0, r1, r2, r4;
	int i;
	unsigned char temp[0x18];
	
	for(i=0; i<=5; i++)
	{
        r0 = i << 1;
        r2 = gethalf(param0+r0);
        sethalf(temp+r0, r2);
        r0 = i + 6;
        r0 = r0 << 1;
        sethalf(temp+r0, r2);
    }
    for(i=5; i>=0; i--)
    {
        r1 = i << 1;
        r0 = i + 4;
        r0 = r0 << 1;
        r0 = gethalf(temp+r0);
        r2 = ~r0;
        r0 = i + 2;
        r0 = r0 << 1;
        r0 = gethalf(temp+r0);
        r0 |= r2;
        r4 = gethalf(param0+r1);
        r0 ^= r4;
        sethalf(param0+r1, r0);
    }
}

void calculo2(unsigned char *param0, unsigned char *param1)
{
	unsigned int r0, r2, r3, r4, r5, r6;
	
	r5 = gethalf(param0);
	r3 = gethalf(param0+2);
	r2 = r3 << 0x10;
	r5 |= r2;
	r4 = gethalf(param0);
	r6 = r4 << 0x10;
	r6 |= r3;
	r2 = r5 >> 0xa;
	sethalf(param0, r2);
	r2 = r6 >> 0xa;
	sethalf(param0+2, r2);
	r5 = gethalf(param1);
	r2 = gethalf(param1+2);
	r0 = r2 << 0x10;
	r5 |= r0;
	r4 = gethalf(param1);
	r6 = r4 << 0x10;
	r6 |= r2;
	r0 = r5 >> 0xF;
	sethalf(param1+2, r0);
	r0 = r6 >> 0xF;
	sethalf(param1, r0);
}

void calculo1b(unsigned char *param0, unsigned char *param1, unsigned char *param2, unsigned char *param3)
{
	unsigned int r0, r1, r4, r5, r6, r8, r9;
	unsigned int temp1, temp2;
	
	r6 = gethalf(param0);
	r4 = r6 << 8;
	r9 = r4;
	r4 ^= r6;
	r5 = gethalf(param0+2);
	r4 ^= r5;
	r8 = r4;
	r4 = r5 << 8;
	r5 ^= r4;
	r5 ^= r6;
	r4 = r9;
	r5 ^= r4;
	r0 = param0[1];
	r5 ^= r0;
	r0 = param1[3];
	r4 = gethalf(param1+2);
	r0 ^= r4;
	r4 = r8;
	r0 ^= r4;
	r0 = r0 << 0x10;
	r0 = r0 >> 0x10;
	r1 = gethalf(param1);
	r1 ^= r5;
	temp1 = r1;
	r4 = r1 << 0x10;
	r4 = r4 >> 0x10;
	temp1 = r4;
	r4 = gethalf(param2+2);
	r0 ^= r4;
	r4 = gethalf(param2);
	r1 = r4 << 8;
	r0 ^= r1;
	r1 = param2[1];
	r0 ^= r1;
	r4 = gethalf(param2+2);
	r4 = r4 << 8;
	temp2 = r4;
	r0 ^= r4;
	r1 = param2[3];
	r0 ^= r1;
	sethalf(param3, r0);
	r0 = gethalf(param2);
	r4 = temp1;
	r0 ^= r4;
	r1 = param2[1];
	r0 ^= r1;
	r4 = gethalf(param2+2);
	r1 = r4 << 8;
	r0 ^= r1;
	r1 = param2[3];
	unk = r1;
	r0 ^= r1;
	sethalf(param3+2, r0);
}

void calculo1a(unsigned char *param0)
{
	int i;
	unsigned char temp[12];
	
	for(i=0; i<12; i++)
	{
        temp[i] = param0[i];
    }
    calculo1b(temp, temp+4, temp+8, param0);
    calculo1b(temp+4, temp+8, temp, param0+4);
    calculo1b(temp+8, temp, temp+4, param0+8);
}

void calculo1(unsigned char *param0, unsigned char *param1, unsigned char *param2)
{
	unsigned int r0, r2, r3, r4;

    r3 = gethalf(param1);
    r4 = gethalf(param0);
    r3 ^= r4;
    sethalf(param1, r3);
    
    r3 = gethalf(param0+2);
    r4 = gethalf(param2);
    r3 ^= r4;
    r4 = gethalf(param1+2);
    r3 ^= r4;
    sethalf(param1+2, r3);
    
    r3 = gethalf(param1+4);
    r4 = gethalf(param0+4);
    r3 ^= r4;
    sethalf(param1+4, r3);

    r3 = gethalf(param1+6);
    r4 = gethalf(param0+6);
    r3 ^= r4;
    sethalf(param1+6, r3);
    
    r4 = gethalf(param0+8);
    r2 = gethalf(param2);
    r4 ^= r2;
    r2 = r4;
    r4 = gethalf(param1+8);
    r2 ^= r4;
    sethalf(param1+8, r2);

    r4 = gethalf(param1+0xa);
    r0 = gethalf(param0+0xa);
    r4 ^= r0;
    r0 = r4;
    sethalf(param1+0xa, r0);
    
    calculo1a(param1);
}

void calc_code(unsigned char *param0, unsigned char *param1, unsigned char *param2)
{
    unsigned int r2;
	int i;

	for(i=0; i<=10; i++)
	{
		r2 = i << 1;
		calculo1(param0, param1, param2+r2);
		calculo2(param1, param1+8);
		calculo3(param1);
		calculo2(param1+8, param1);
	}

    calculo1(param0, param1, param2+0x16);
	calculo4(param1);
}

int main(int argc, char *argv[])
{
	unsigned char mi_tabla1[24], mi_tabla2[12];
	unsigned int r0, r1;
	int i;
	unsigned char imei_hex[12];
	unsigned char destino[11];
	unsigned char *imei;
	
    if (argc < 2)
    {
       printf("Use: %s imei\n", argv[0]);
       return -1;
    }

	imei = argv[1];

	if (strlen(imei) != 15)
	   return -1;

	memcpy(mi_tabla1, tabla1, 24);
	memcpy(mi_tabla2, tabla2, 12);

	sethalf(imei_hex, 0);
	sethalf(imei_hex+0x8, 0);
	sethalf(imei_hex+0xa, 0);

	for(i=0; i<=6; i++)
	{
		r0 = i << 1;
		r1 = imei[r0];
		r1 &= 0x0F;
		r1 = r1 << 4;
		r0 = imei[r0+1];
		r0 &= 0x0F;
		r1 |= r0;
		imei_hex[i+2] = (char)r1;
	}

	inv(imei_hex);
	calc_code(mi_tabla2, imei_hex, mi_tabla1);
	inv(imei_hex);
	
	for(i=0; i<=4; i++)
	{
        r1 = i << 1;
        r0 = imei_hex[i];
        r0 = r0 >> 4;
        r0 &= 7;
        r0 += 0x30;
        destino[r1] = (char)r0;
        r0 = imei_hex[i];
        r0 &= 7;
        r0 += 0x30;
        destino[r1+1] = (char)r0;
    }
    destino[r1+2] = 0;
    printf("%u\n",r1+2);
    
    printf("IMEI: %s\n", imei);
  	printf("mastercode: %s\n", destino);
        
	return 0;
}