hci.cxx

三星2440原版bsp

				gpHCI->Unlock ();
				break;
			}

			pPacket = gpHCI->pPacketsSent;
			pParent = NULL;
			while (pPacket && (pPacket->ePacketType == COMMAND_PACKET)) {
				pParent = pPacket;
				pPacket = pPacket->pNext;
			}

			if (! pPacket) {
				gpHCI->Unlock ();
				break;
			}

			//	There's pending data packet in SentPackets.
			HCI_CONTEXT *pOwner = VerifyContext (pPacket->pOwner);
			void *pCallContext = pPacket->pCallContext;

			HCI_DataPacketDown_Out pCallback = pOwner ? pOwner->c.hci_DataPacketDown_Out : NULL;

			if (! pParent)
				gpHCI->pPacketsSent = pPacket->pNext;
			else
				pParent->pNext = pPacket->pNext;

			delete pPacket;

			if (pCallback) {
				gpHCI->AddRef ();
				IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"Data packet completed:: going into callback - 2\n"));
				gpHCI->Unlock ();

				__try {
					pCallback (pCallContext, ERROR_SUCCESS);
				} __except (1) {
					IFDBG(DebugOut (DEBUG_ERROR, L"Data packet completed:: exception in callback - 2\n"));
				}

				gpHCI->Lock ();
				IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"Data packet completed:: came from callback - 2\n"));
				gpHCI->Release ();
			}
			gpHCI->Unlock ();
		}

		SetEvent (hEvent);	// Rescan for packets...
	}

	return 0;
}

static void DataUp (BasebandConnection *pC, BD_BUFFER *pB, HCI_CONTEXT *pOwner) {
	if (! pOwner)
		pOwner = VerifyContext (pC->pOwner);

	if (! pOwner) {
		IFDBG(DebugOut (DEBUG_WARN, L"DataUp: no context for data packet!\n"));
		return;
	}

	HCI_DataPacketUp pCallback = pOwner->ei.hci_DataPacketUp;

	if (! pCallback) {
		IFDBG(DebugOut (DEBUG_WARN, L"DataUp: no callback for data packet!\n"));
		return;
	}

	void *pUserContext = pC->pOwner->pUserContext;
	unsigned short h = pC->connection_handle;

	gpHCI->AddRef ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"DataUp:: going into callback\n"));
	gpHCI->Unlock ();

	__try {
		pCallback (pUserContext, h, pB);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"DataUp:: exception in callback\n"));
	}

	gpHCI->Lock ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"DataUp:: came from callback\n"));
	gpHCI->Release ();
}

static void DataUpAclUnbuffered (BasebandConnection *pC, unsigned char boundary, unsigned char broadcast, BD_BUFFER *pB, HCI_CONTEXT *pOwner) {
	HCI_DataPacketUpAclUnbuffered pCallback = pOwner->ei.hci_DataPacketUpAclUnbuffered;

	void *pUserContext = pC->pOwner->pUserContext;
	unsigned short h = pC->connection_handle;

	gpHCI->AddRef ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"DataUpAclUnbuffered:: going into callback\n"));
	gpHCI->Unlock ();

	__try {
		pCallback (pUserContext, h, boundary, broadcast, pB);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"DataUpAclUnbuffered:: exception in callback\n"));
	}

	gpHCI->Lock ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"DataUpAclUnbuffered:: came from callback\n"));
	gpHCI->Release ();
}

static void ProcessDataAcl (unsigned char *pBuffer, int cSize) {
	IFDBG(DebugOut (DEBUG_HCI_PACKETS, L"RT: Received ACL data packet\n"));

	unsigned short	hConnection = pBuffer[0] | ((pBuffer[1] & 0xf) << 8);
	int				fStart = (pBuffer[1] >> 4) & 0x3;
	int				cLength = pBuffer[2] | (pBuffer[3] << 8);

	if ((cSize < 4) || (cLength + 4 != cSize)) {	// at least header should fit...
		IFDBG(DebugOut (DEBUG_ERROR, L"RT: Data packet too small (%d bytes)... flushed.\n", cSize));
		IncrHWErr ();
		return;
	}

	BasebandConnection *pC = FindConnection (hConnection);

	if (! pC) {
		IFDBG(DebugOut (DEBUG_WARN, L"ProcessDataAcl: Connection handle %08x does not exist... Packet flushed.\n", hConnection));
		return;
	}

	HCI_CONTEXT *pOwner = VerifyContext (pC->pOwner);

	if (pOwner && pOwner->ei.hci_DataPacketUpAclUnbuffered) {
		IFDBG(DebugOut (DEBUG_HCI_PACKETS, L"RT: Acl packet arrived from BD_ADDR = %04x%08x - sending unbuffered...\n", pC->ba.NAP, pC->ba.SAP));
		BD_BUFFER b;

		b.fMustCopy = TRUE;
		b.pFree = BufferFree;

		b.cSize = cLength;
		b.cEnd = b.cSize;
		b.cStart = 0;

		b.pBuffer = pBuffer + 4;

		DataUpAclUnbuffered (pC, fStart, (pBuffer[1] >> 6) & 0x3, &b, pOwner);
		return;
	}

	if (fStart == 1) { // Continuation
		if (pC->pAclPacket) {
			if (cLength + pC->cAclBytesComplete > pC->pAclPacket->cSize) {	// Too many bytes...
				IFDBG(DebugOut (DEBUG_WARN, L"RT: Packet overflow from BD_ADDR = %04x%08x... Discarding everything\n", pC->ba.NAP, pC->ba.SAP));
				pC->pAclPacket->pFree (pC->pAclPacket);
				pC->pAclPacket = NULL;
				pC->cAclBytesComplete = 0;
				return;
			}

			memcpy (&pC->pAclPacket->pBuffer[pC->cAclBytesComplete], pBuffer + 4, cLength);
			pC->cAclBytesComplete += cLength;

			if (pC->cAclBytesComplete == pC->pAclPacket->cSize) { // send DataUp
				BD_BUFFER *pB = pC->pAclPacket;

				pC->cAclBytesComplete = 0;
				pC->pAclPacket = NULL;
				DataUp (pC, pB, pOwner);
			}
		} else	// ...without the beginning...
			IFDBG(DebugOut (DEBUG_WARN, L"ProcessDataAcl: Cont. arrived with no start from BD_ADDR = %04x%08x... Packet flushed\n", pC->ba.NAP, pC->ba.SAP));
		return;
	} else if (fStart != 2) {
		IFDBG(DebugOut (DEBUG_WARN, L"RT: Packet marker unknown... flushed.\n"));
		IncrHWErr ();
		return;
	}

	if (pC->pAclPacket) {	// remains are flushed
		IFDBG(DebugOut (DEBUG_WARN, L"RT: Packet overflow from BD_ADDR = %04x%08x... Discarding everything\n", pC->ba.NAP, pC->ba.SAP));
		pC->pAclPacket->pFree (pC->pAclPacket);
		pC->pAclPacket = NULL;
		pC->cAclBytesComplete = 0;
	}

					// start new packet.
	SVSUTIL_ASSERT (! pC->pAclPacket);
	SVSUTIL_ASSERT (! pC->cAclBytesComplete);

	if (cLength < 4) {
		IFDBG(DebugOut (DEBUG_WARN, L"RT: First data packet smaller (%d) than ACL header BD_ADDR = %04x%08x... Packet flushed\n", cLength, pC->ba.NAP, pC->ba.SAP));
		return;
	}

	int cAclLen = *(unsigned short *)&pBuffer[4] + 4;
	if (cAclLen == cLength) {		// Unfragmented Acl packet
		IFDBG(DebugOut (DEBUG_HCI_PACKETS, L"RT: Acl packet (unfragmented) arrived from BD_ADDR = %04x%08x...\n", pC->ba.NAP, pC->ba.SAP));
		BD_BUFFER b;

		b.fMustCopy = TRUE;
		b.pFree = BufferFree;

		b.cSize = cLength;
		b.cEnd = b.cSize;
		b.cStart = 0;

		b.pBuffer = pBuffer + 4;

		DataUp (pC, &b, pOwner);
		return;
	}

	if (cLength > cAclLen) {	// If packet is BIGGER than ACL packet, it is really bad...
		IFDBG(DebugOut (DEBUG_ERROR, L"RT: Data packet too big (ACL: %d bytes, data: %d)... flushed.\n", cAclLen, cLength));
		IncrHWErr ();
		return;
	}

	pC->pAclPacket = BufferAlloc (cAclLen);
	if (! pC->pAclPacket) {
		IFDBG(DebugOut (DEBUG_ERROR, L"RT: OOM allocating %d bytes BD_ADDR = %04x%08x... Packet flushed\n", cAclLen, pC->ba.NAP, pC->ba.SAP));
		return;
	}

	memcpy (pC->pAclPacket->pBuffer, pBuffer + 4, cLength);
	pC->cAclBytesComplete = cLength;
}

static void ProcessDataSco (unsigned char *pBuffer, int cSize) {
	IFDBG(DebugOut (DEBUG_HCI_PACKETS, L"RT: Received SCO data packet\n"));

	unsigned short	hConnection = pBuffer[0] | ((pBuffer[1] & 0xf) << 8);
	int				cLength = pBuffer[2];

	if ((cSize < 3) || (cLength + 3 != cSize)) {	// at least header should fit...
		IFDBG(DebugOut (DEBUG_ERROR, L"RT: Data packet too small (%d bytes)... flushed.\n", cSize));
		IncrHWErr ();
		return;
	}

	BasebandConnection *pC = FindConnection (hConnection);

	if (! pC) {
		IFDBG(DebugOut (DEBUG_WARN, L"ProcessDataSco: Connection handle %08x does not exist... Packet flushed.\n", hConnection));
		return;
	}

	IFDBG(DebugOut (DEBUG_HCI_PACKETS, L"RT: Sco packet arrived from BD_ADDR = %04x%08x...\n", pC->ba.NAP, pC->ba.SAP));

	BD_BUFFER b;

	b.fMustCopy = TRUE;
	b.pFree = BufferFree;

	b.cSize = cLength;
	b.cEnd = b.cSize;
	b.cStart = 0;

	b.pBuffer = pBuffer + 3;

	DataUp (pC, &b, NULL);
}

// Generated in response to HCI_Reject_Connection_Request,
//							HCI_Accept_Connection_Request,
//							HCI_Create_Connection,
//							HCI_Add_SCO_Connection
static void ConnectionComplete (unsigned char status, unsigned short h, BD_ADDR *pba, unsigned char link_type, unsigned char encryption_mode) {
	IFDBG(DebugOut (DEBUG_HCI_TRACE, L"ConnectionComplete status = %d handle = 0x%04x bd_addr = %04x%08x link = %d encrypt = %d\n", status, h, pba->NAP, pba->SAP, link_type, encryption_mode));

	HCIPacket *pPacket = gpHCI->pPacketsPending;
	HCIPacket *pParent = NULL;
	while (pPacket) {
		if (pPacket->ePacketType == COMMAND_PACKET) {
			SVSUTIL_ASSERT (pPacket->pContents->cSize >= 3);
			SVSUTIL_ASSERT (pPacket->pContents->cEnd == pPacket->pContents->cSize);
			SVSUTIL_ASSERT (pPacket->pContents->cStart == 0);
			SVSUTIL_ASSERT (pPacket->pContents->pBuffer[2] + 3 == pPacket->pContents->cSize);

			if ((pPacket->uPacket.CommandPacket.opCode == HCI_Create_Connection) ||
				(pPacket->uPacket.CommandPacket.opCode == HCI_Accept_Connection_Request) ||
				(pPacket->uPacket.CommandPacket.opCode == HCI_Reject_Connection_Request)) {
				SVSUTIL_ASSERT (pPacket->uPacket.CommandPacket.m.fMarker == BTH_MARKER_ADDRESS);
				if (*pba == pPacket->uPacket.CommandPacket.m.ba)
					break;
			} else if (pPacket->uPacket.CommandPacket.opCode == HCI_Add_SCO_Connection) {
				SVSUTIL_ASSERT (pPacket->uPacket.CommandPacket.m.fMarker == BTH_MARKER_CONNECTION);
				BasebandConnection *pC = FindConnection (pPacket->uPacket.CommandPacket.m.connection_handle);
				if (pC && (pC->ba == *pba)) // Unaligned!
					break;
			}
		}
		pParent = pPacket;
		pPacket = pPacket->pNext;
	}

	if (! pPacket) {
		IFDBG(DebugOut (DEBUG_WARN, L"Orphaned connection complete event (h = 0x%08x, ba = %04x%08x)\n", h, pba->NAP, pba->SAP));
		return;
	}

	unsigned int remote_cod = 0;
	
	if ((pPacket->uPacket.CommandPacket.opCode == HCI_Accept_Connection_Request) ||
		(pPacket->uPacket.CommandPacket.opCode == HCI_Reject_Connection_Request)) {
		ConnReqData *pCR = gpHCI->pConnReqData;
		ConnReqData *pPrev = NULL;
		while (pCR && (pCR->ba != *pba) && pCR->cod) {
			pPrev = pCR;
			pCR = pCR->pNext;
		}

		if (pCR) {
			remote_cod = pCR->cod;

			if (pPrev)
				pPrev->pNext = pCR->pNext;	
			else
				gpHCI->pConnReqData = pCR->pNext;

			delete pCR;
		}
	} else if (pPacket->uPacket.CommandPacket.opCode == HCI_Create_Connection) {
		InquiryResultList *pInq = gpHCI->pInquiryResults;
		while (pInq && (pInq->irb.ba != *pba))
			pInq = pInq->pNext;

		if (pInq)
			remote_cod = pInq->irb.class_of_device;
	}
	
	if (! pParent)
		gpHCI->pPacketsPending = gpHCI->pPacketsPending->pNext;
	else
		pParent->pNext = pPacket->pNext;

	HCI_CONTEXT *pOwner = VerifyContext (pPacket->pOwner);
	void *pCallContext = pPacket->pCallContext;

	delete pPacket;

	if (! pOwner) {
		IFDBG(DebugOut (DEBUG_WARN, L"Orphaned connection complete event.\n"));
		return;
	}

	if (status == 0) {
#if defined (DEBUG) || defined (_DEBUG)
		{
			BasebandConnection *pRunner = gpHCI->pConnections;
			while (pRunner) {
				SVSUTIL_ASSERT (pRunner->connection_handle != h);
				pRunner = pRunner->pNext;
			}
		}
#endif
		BasebandConnection *pNewC = new BasebandConnection;

		if (pNewC) {
			memset (pNewC, 0, sizeof(*pNewC));
			pNewC->ba = *pba;
			pNewC->pOwner = pOwner;
			pNewC->connection_handle = h;
			pNewC->remote_cod = remote_cod;
			pNewC->link_type = link_type;
			pNewC->pNext = gpHCI->pConnections;
			gpHCI->pConnections = pNewC;
		}
	}

	HCI_ConnectionCompleteEvent pCallback = pOwner->ei.hci_ConnectionCompleteEvent;

	if (! pCallback) {
		IFDBG(DebugOut (DEBUG_WARN, L"ConnectionComplete:: no handler\n"));
		return;
	}

	void *pUserContext = pOwner->pUserContext;

	gpHCI->AddRef ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"ConnectionComplete:: going into callback\n"));
	gpHCI->Unlock ();

	__try {
		pCallback (pUserContext, pCallContext, status, h, pba, link_type, encryption_mode);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"ConnectionComplete:: exception in callback\n"));
	}

	gpHCI->Lock ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"ConnectionComplete:: came from callback\n"));
	gpHCI->Release ();
}

static void InquiryComplete (unsigned char status, unsigned char num_responses) {
	IFDBG(DebugOut (DEBUG_HCI_TRACE, L"InquiryComplete status = %d num = %d\n", status, num_responses));

	HCIPacket *pPacket = ExtractCommandPacket (&gpHCI->pPacketsPending, HCI_Inquiry);

	HCI_CONTEXT *pOwner = NULL;
	void *pCallContext = NULL;
	if (pPacket) {
		pOwner = VerifyContext (pPacket->pOwner);
		pCallContext = pPacket->pCallContext;

		delete pPacket;
	} else
		pOwner = gpHCI->pPeriodicInquiryOwner;

	if (! pOwner) {
		IFDBG(DebugOut (DEBUG_WARN, L"Orphaned inquiry complete event (no owner).\n"));
		return;
	}

	HCI_InquiryCompleteEvent pCallback = pOwner->ei.hci_InquiryCompleteEvent;

	if (! pCallback) {
		IFDBG(DebugOut (DEBUG_WARN, L"InquiryComplete:: no handler\n"));
		return;
	}

	void *pUserContext = pOwner->pUserContext;

	gpHCI->AddRef ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"InquiryComplete:: going into callback\n"));
	gpHCI->Unlock ();

	__try {
		pCallback (pUserContext, pCallContext, status);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"InquiryComplete:: exception in callback\n"));
	}

	gpHCI->Lock ();
	IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"InquiryComplete:: came from callback\n"));
	gpHCI->Release ();
}