#include "stdafx.h"
#include "debug.h"
#include "gsmdevice.h"
#include "unlocker.h"

Unlocker::Unlocker()
{
	// test unscrambler
	DWORD scrambled[]= {0x8C270404,0x2602B2C1,0xC62D3120,0x106322CA,0xD40240CC,0xA2A2D22A,0x06493000};
	DWORD unscrambled[]= {0x82460213,0x58916301,0x98609316,0x91096431,0x22686601,0x69511551,0x98000324 };

	for (int i=0 ; i<sizeof(scrambled)/sizeof(DWORD) ; i++)
	{
		DWORD dwUnScrambled= UnScrambleCode(scrambled[i]);
		
		debug("%08lx -> %08lx %hs %08lx\n", scrambled[i], 
			dwUnScrambled, 
			dwUnScrambled==unscrambled[i]?"==":"!=",
			unscrambled[i]);
	}

	m_gsm.open();
}
Unlocker::~Unlocker()
{
}

bool Unlocker::DoSidLck(bool bLock, CString& unlockcode)
{
	CString sidlck;
	sidlck.Format(L"at%%sidlck=%d,%s\r", bLock?1:0, unlockcode.GetBuffer(0));
	if (!m_gsm.send(sidlck))
		return false;

	CString reply;
	if (!m_gsm.receive(reply))
		return false;

	debug("unlock reply: %ls\n", reply.GetBuffer(0));

	// ... mmm actually this might never find the error, since the phone is in 'atv0' mode
	if (-1 == reply.Find(L"ERROR"))
		return true;

	return false;
}
bool Unlocker::DoUnlock(CString& unlockcode)
{
	return DoSidLck(0, unlockcode);
}
bool Unlocker::LockPhone(CString& unlockcode)
{
	return DoSidLck(1, unlockcode);
}

bool Unlocker::GetUnlockCode(CString& unlockcode)
{
	if (!m_gsm.send(CString("AT%UREG?3fe00c,4\r")))
		return false;

	CString reply;
	if (!m_gsm.receive(reply))
		return false;

	debug("try1: %ls\n", reply.GetBuffer(0));

	int uregpos= reply.Find(L"EXT_UREG");
	if (uregpos>=0 && reply.GetLength()>=uregpos+17)
	{
		unlockcode= reply.Mid(uregpos+9, 8);
		return true;
	}

	if (!m_gsm.send(CString("AT%UREG?3FE004,FFFFFFFF\r")))
		return false;

	if (!m_gsm.receive(reply))
		return false;

	debug("try2: %ls\n", reply.GetBuffer(0));

	if (reply.GetLength() < 200)
	{
		debug("reply too short\n");
		return false;
	}
	uregpos= reply.Find(L"EXT_UREG");
	if (uregpos==-1) {
		debug("ext_ureg not found\n");
		return false;
	}
	CString scrambledcode= reply.Mid(uregpos+161, 8);
	CString scrambled2= reply.Mid(uregpos+161-4, 4)+reply.Mid(uregpos+161+8,4);
	if (scrambled2!=scrambledcode)
	{
		debug("abab pattern not found\n");
		return false;
	}
	DWORD dwScrambled= _tcstoul(scrambledcode.GetBuffer(0), 0, 16);

	DWORD dwUnScrambled= UnScrambleCode(dwScrambled);

	unlockcode.Format(L"%08lX", dwUnScrambled);

	return true;
}

DWORD Unlocker::UnScrambleCode(DWORD dwScrambled)
{
	int permutation[]= {2,5,6,7,0,3,4,1};

	DWORD dwUnScrambled= 0;
	for (int i=0 ; i<8 ; i++) {
		BYTE n= (BYTE)(dwScrambled>>28)&0xf;
		dwUnScrambled |= n<<(4*(7-permutation[i]));

		dwScrambled<<=4;
	}

	DWORD bitmask= 0x08080800;
	DWORD bits= dwUnScrambled&bitmask;
	dwUnScrambled = (dwUnScrambled&~bitmask) | (bits<<8) | ((bits>>16)&0xff00);
	dwUnScrambled = (dwUnScrambled >> 1) | (dwUnScrambled <<31);

	return dwUnScrambled;
}