hcibcsp.cpp

Windows CE操作系统中适用的蓝牙驱动程序

#endif
		CloseHandle (hFile);
	}

	hFile = INVALID_HANDLE_VALUE;
}

//
//	SLIP coding
//
static int GetSLIPEncodedLength(const void *pvData, int len) {
	const unsigned char *pData = (unsigned char *)pvData;
	int slip_len = len;

	for (int i = 0; i < len; ++i) {
		if((pData[i] == SLIP_END) || (pData[i] == SLIP_ESC))
			slip_len += 1;
	}

	return slip_len;
}

static void SLIPEncodeIntoBuffer (const void *pvSourceData, int sourceLen, unsigned char *pTargetData) {
	const unsigned char *pSourceData = (unsigned char *)pvSourceData;

	for(int i = 0; i < sourceLen; ++i) {
		unsigned char byte = pSourceData[i];

		switch(byte) {

		case SLIP_END:
			*pTargetData++ = SLIP_ESC;
			*pTargetData++ = SLIP_ESC_END;
			break;

		case SLIP_ESC:
			*pTargetData++ = SLIP_ESC;
			*pTargetData++ = SLIP_ESC_ESC;
			break;

		default:
			*pTargetData++ = pSourceData[i];
			break;

		}
	}
}

static int SLIPReadBytes(void *pvData, int len, BOOL* pfSlipError) {
	unsigned char *pData = (unsigned char *)pvData;
	unsigned char slipBuffer[SLIP_BUFFER_SIZE];
	int unslippedByte = 0;

	*pfSlipError = FALSE;
	
	while (unslippedByte < len) {
		int bytesRead = len - unslippedByte;

		if (bytesRead > SLIP_BUFFER_SIZE)
			bytesRead = SLIP_BUFFER_SIZE;
		
		if (! ReadCommPort(slipBuffer, bytesRead)) {
			IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] SLIPReadBytes failed.\n"));

			return FALSE;
		}

		int slipBufferPos = 0;

		while (slipBufferPos < bytesRead) {
			unsigned char byte = slipBuffer[slipBufferPos++];
			if(byte == SLIP_ESC) {
				if(slipBufferPos == bytesRead) {
					if (! ReadCommPort (&byte, 1)) {
						IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] SLIPReadBytes failed.\n"));

						return FALSE;
					}
				} else
					byte = slipBuffer[slipBufferPos++];

				pData[unslippedByte++] = (byte == SLIP_ESC_ESC) ? SLIP_ESC : SLIP_END;
			} else if (byte == SLIP_END) {
				*pfSlipError = TRUE;
				IFDBG(DebugOut(DEBUG_WARN, L"[BCSP] SLIPReadBytes failed with SLIP error.\n"));
				PushBackData(&slipBuffer[slipBufferPos], bytesRead-slipBufferPos);
				return FALSE;
			} else
				pData[unslippedByte++] = byte;	

			ASSERT (unslippedByte <= len);
		}
	}
	
	return TRUE;
}

static int ReadFromCOMPort(BCSPHeader *pHeader, unsigned char *pPayloadData) {
  	BOOL fDone = FALSE;

	while (!fDone) {
		// locate the start of a new packet
		unsigned char byte = 0;
		int fReadOK = TRUE;
		BOOL fSlipError = FALSE;

		while ((fReadOK = ReadCommPort (&byte, 1)) && (byte != SLIP_END))
			IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN : skip <%02x>\n", byte));

		if (! fReadOK) {
			IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] ReadFromCOMPort failed.\n"));
			return FALSE;
		}

		while ((byte == SLIP_END) && (fReadOK = ReadCommPort (&byte, 1)))
			IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN : skip <SLIP_END>\n"));

		if (! fReadOK) {
			IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] ReadFromCOMPort failed.\n"));
			return FALSE;
		}

		
		// now byte holds the first byte of the header structure
		// put it in there and read the rest of the header

		if(byte == SLIP_ESC) {
			if (! ReadCommPort (&byte, 1))
				return FALSE;

			pHeader->flags = (byte == SLIP_ESC_END) ? SLIP_END : SLIP_ESC;	
		} else pHeader->flags = byte;

		if (! SLIPReadBytes (((unsigned char *)pHeader) + 1, sizeof(*pHeader) - 1, &fSlipError)) {
			if (fSlipError)
				continue;

			IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] ReadFromCOMPort failed.\n"));
			return FALSE;
		}

#if defined (FULL_BCSP_DUMP)
		IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: seq      = %d\n", pHeader->seq));
		IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: ack      = %d\n", pHeader->ack));
		IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: crc      = %s\n", pHeader->crcPresent ? L"yes" : L"no"));
		IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: protocol = %d\n", pHeader->protocolType));
		IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: prot. id = %d\n", pHeader->protocolID));
		IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: payload  = %d bytes\n", pHeader->payloadLength));
#endif

		if ((pHeader->payloadLength > 0) && (! SLIPReadBytes(pPayloadData, pHeader->payloadLength, &fSlipError))) {
			if (fSlipError)
				continue;

			IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] ReadFromCOMPort failed.\n"));
			return FALSE;
		}

#if defined (FULL_BCSP_DUMP)
		IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: HEX HEADER\n"));
		IFDBG(for (int i = 0 ; i < sizeof (*pHeader) ; ++i) DebugOut (DEBUG_OUTPUT, L"%02x ", ((unsigned char *)pHeader)[i]););
		IFDBG(DebugOut (DEBUG_OUTPUT, L"\n"));

		if (pHeader->payloadLength > 0) {
			IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: PAYLOAD\n", pHeader->payloadLength));
			IFDBG(DumpBuff (DEBUG_HCI_TRANSPORT, pPayloadData, pHeader->payloadLength));
		}
#endif

		unsigned short crc;

		if(pHeader->crcPresent) {
			SLIPReadBytes (&crc, sizeof(crc), &fSlipError);
#if defined (FULL_BCSP_DUMP)
			IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET IN: <crc:%02x>\n", crc));
#endif
		}

		fReadOK = ReadCommPort (&byte, 1);	
		if (! fReadOK) {
			IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] ReadFromCOMPort failed.\n"));
			return FALSE;
		}

		if (byte != SLIP_END)
			continue;

		fDone = TRUE;
	}

	return TRUE;
}

//
//	Misc class support functions
//
//
//	BCSPPacket
//

unsigned char* g_pWriteBuff;

int BCSPPacket::WriteToCOMPort (int fUnlock) {
	header.SetChecksum ();

	// first SLIP-encode header into packet data
	int slipHeaderLength = GetSLIPEncodedLength (&header, sizeof(header));

	ASSERT (slipHeaderLength <= 9);

	SLIPEncodeIntoBuffer (&header, sizeof(header), &pPacketData[9 - slipHeaderLength]);

	// write SLIP_END bytes
	pPacketData[8 - slipHeaderLength] = pPacketData[slipPayloadLength + 10 - 1] = SLIP_END;

#if defined (FULL_BCSP_DUMP)
	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET OUT: seq      = %d\n", header.seq));
	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET OUT: ack      = %d\n", header.ack));
	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET OUT: crc      = %s\n", header.crcPresent ? L"yes" : L"no"));
	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET OUT: protocol = %d\n", header.protocolType));
	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET OUT: prot. id = %d\n", header.protocolID));
	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET OUT: payload  = %d bytes\n", header.payloadLength));

	IFDBG(DebugOut(DEBUG_HCI_TRANSPORT, L"[BCSP] PACKET OUT: CODED PAYLOAD\n"));
	IFDBG(DumpBuff(DEBUG_HCI_TRANSPORT, &pPacketData[8 - slipHeaderLength], slipPayloadLength + slipHeaderLength + 2));
#endif

	int cBuffer = slipPayloadLength + slipHeaderLength + 2;

	ASSERT (cBuffer < MAX_BUFFER_SIZE);
	ASSERT (g_pWriteBuff);
	
	memcpy (g_pWriteBuff, &pPacketData[8 - slipHeaderLength], cBuffer);

	if (fUnlock)
		LeaveCriticalSection (&g_csBCSP);

	int iRes = WriteCommPort(g_pWriteBuff, cBuffer);

	if (fUnlock)
		EnterCriticalSection (&g_csBCSP);

	if (! iRes) {
		IFDBG(DebugOut(DEBUG_ERROR, L"[BCSP] BCSPPacket::WriteToCOMPort failed.\n"));
	}

	return iRes;
}

void BCSPPacket::SetData (const void *pvData, int len) {
	if(pPacketData)
		free(pPacketData);

	pPacketData = 0;
	
	header.payloadLength = len;
	slipPayloadLength = GetSLIPEncodedLength (pvData, len);

	pPacketData = (unsigned char *)malloc (slipPayloadLength + 10);
	if (pPacketData) {
		SLIPEncodeIntoBuffer (pvData, len, &pPacketData[9]);
	}
}

//
//	BCSPPacketListNode
//
static int GetListSize (void) {
	BCSPPacketListNode *pPacket = g_pTransmitPackets;
	
	int size = 0;
	while(pPacket) {
		++size;
		pPacket = pPacket->next;
	}

	return size;
}

static int GetSCOListSize (void) {
	BCSPPacketListNode *pPacket = g_pSCOTransmitPackets;
	
	int size = 0;
	while(pPacket) {
		++size;
		pPacket = pPacket->next;
	}

	return size;
}

static void AddToList(BCSPPacketListNode* pNewPacket) {
	BCSPPacketListNode* pPacket;
	
	ASSERT (pNewPacket->next == 0);

	if (pNewPacket->packet.header.protocolID == IO_DATA_PROTOCOL_HCI_SCO) {
		if(! g_pSCOTransmitPackets) {
			g_pSCOTransmitPackets = pNewPacket;
			return;
		}
		pPacket = g_pSCOTransmitPackets;
	}
	else {
		if(! g_pTransmitPackets) {
			g_pTransmitPackets = pNewPacket;
			return;
		}
		pPacket = g_pTransmitPackets;
	}

	while(pPacket->next) pPacket = pPacket->next;
	pPacket->next = pNewPacket;
}

static BCSPPacketListNode *GetFirstPacketForSend (void) {
	BCSPPacketListNode *pPacket = g_pTransmitPackets;
	while(pPacket && (! pPacket->packet.fResendPacket) && (! pPacket->packet.fNewPacket))
		pPacket = pPacket->next;

	return pPacket;
}

static BCSPPacketListNode *GetNextSCOPacket (void) {
	BCSPPacketListNode *pPacket = g_pSCOTransmitPackets;
	if (g_pSCOTransmitPackets) {
		g_pSCOTransmitPackets = g_pSCOTransmitPackets->next;
	}
	
	return pPacket;
}

//
//	Protocol procedures : Link establishment
//
//			HANDLE handles[2] = { hLinkEstablishmentThreadDie, hLinkEstablishmentSyncReceived };
static DWORD WINAPI BCSPLinkEstablishmentThread (LPVOID phandles) {
	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] BCSPLinkEstablishmentThread started...\n"));

	const unsigned char syncPayload[] = { 0xDA, 0xDC, 0xED, 0xED };

	BCSPPacket syncPacket;
	syncPacket.SetData(syncPayload, 4);
	syncPacket.header.ack = 0;
	syncPacket.header.seq = 0;
	syncPacket.header.crcPresent = 0;
	syncPacket.header.payloadLength = 4;
	syncPacket.header.protocolID = 1;
	syncPacket.header.protocolType = 0;

	const unsigned char syncRespPayload[] = { 0xAC, 0xAF, 0xEF, 0xEE };

	BCSPPacket syncRespPacket;
	syncRespPacket.SetData(syncRespPayload, 4);
	syncRespPacket.header.ack = 0;
	syncRespPacket.header.seq = 0;
	syncRespPacket.header.crcPresent = 0;
	syncRespPacket.header.payloadLength = 4;
	syncRespPacket.header.protocolID = 1;
	syncRespPacket.header.protocolType = 0;

	const unsigned char confPayload[] = { 0xAD, 0xEF, 0xAC, 0xED };

	BCSPPacket confPacket;
	confPacket.SetData(confPayload, 4);
	confPacket.header.ack = 0;
	confPacket.header.seq = 0;
	confPacket.header.crcPresent = 0;
	confPacket.header.payloadLength = 4;
	confPacket.header.protocolID = 1;
	confPacket.header.protocolType = 0;

	IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] BCSPLinkEstablishmentThread : SYNC\n"));

	if (syncPacket.WriteToCOMPort (FALSE)) {
		int iCount = 0;

		while (iCount < BCSP_RETRY_LIMIT) {
			DWORD ret = WaitForMultipleObjects (2, (HANDLE *)phandles, FALSE, BCSP_PASSIVE_TIMEOUT);

			if(WAIT_OBJECT_0 == ret) {
				IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] BCSPLinkEstablishmentThread : got SYNC-RESP, sending CONF and returning\n"));
				return confPacket.WriteToCOMPort (FALSE);
			} else if (WAIT_OBJECT_0 + 1 == ret) {
				IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] BCSPLinkEstablishmentThread : got SYNC, send SYNC-RESP\n"));

				if (! syncRespPacket.WriteToCOMPort (FALSE))
					break;
			} else {
				ASSERT (ret == WAIT_TIMEOUT);
				IFDBG(DebugOut (DEBUG_HCI_TRANSPORT, L"[BCSP] BCSPLinkEstablishmentThread : timeout, resending SYNC\n"));

				if (! syncPacket.WriteToCOMPort (FALSE))
					break;

				++iCount;
			}
		}
	}

	IFDBG(DebugOut (DEBUG_ERROR, L"[BCSP] BCSPLinkEstablishmentThread : closing port and dying\n"));
	CloseCommPort ();

	return FALSE;
}

static int BCSPLinkEstablishment (void) {
#ifndef BCSP_NO_LINK_ESTABLISHMENT
	unsigned char payload[PACKET_SIZE_R];
	BCSPHeader syncHeader;

	const unsigned char syncPayload[] = { 0xDA, 0xDC, 0xED, 0xED };
	const unsigned char syncRespPayload[] = { 0xAC, 0xAF, 0xEF, 0xEE };

	payload[0] = 0;

	HANDLE hLinkEstablishmentThreadDie    = CreateEvent (NULL, TRUE, FALSE, NULL);
	HANDLE hLinkEstablishmentSyncReceived = CreateEvent (NULL, FALSE, FALSE, NULL);

	HANDLE handles[2] = { hLinkEstablishmentThreadDie, hLinkEstablishmentSyncReceived };

	HANDLE hThread = CreateThread(NULL, 0, BCSPLinkEstablishmentThread, handles, 0, NULL);

	int iRes = FALSE;

	while (TRUE) {
		if (! ReadFromCOMPort(&syncHeader, payload))
			break;

		if (syncHeader.payloadLength != 4)
			continue;

		if (0 == memcmp(syncPayload, payload, 4))
			SetEvent(hLinkEstablishmentSyncReceived);
		else if (0 == memcmp(syncRespPayload, payload, 4)) {
			iRes = TRUE;
			break;
		}
	}