l2cap.cxx

三星2440原版bsp

			InquiryResultBuffer irb;
			int cSizeRet = 0;
			iRes = pIoctl (hHCI, BTH_HCI_IOCTL_GET_LAST_INQUIRY_DATA, sizeof(b), (char *)&b,
						sizeof(irb), (char *)&irb, &cSizeRet);

			if ((iRes != ERROR_SUCCESS) || (cSizeRet != sizeof(irb)))
				memset (&irb, 0, sizeof(irb));

			iRes = pHCICall (hHCI, pCall, &b, usPacketType, irb.page_scan_repetition_mode, irb.page_scan_mode, irb.clock_offset, 1);
		} __except (1) {
			IFDBG(DebugOut (DEBUG_ERROR, L"exception in hci_CreateConnection_In\n"));
		}
		gpL2CAP->Lock ();
		gpL2CAP->DelRef ();
		IFDBG(DebugOut (DEBUG_HCI_CALLBACK, L"came out of hci_CreateConnection_In\n"));
	} else
		IFDBG(DebugOut (DEBUG_ERROR, L"[L2CAP] DisconnectPhysicalLink :: Call not matched!\n"));

	if (iRes != ERROR_SUCCESS)
		return DisconnectPhysicalLink (pPhysLink, FALSE, iRes, NULL);

	return iRes;
}

static int RequestPhysicalLink (CallContext *pCall) {
	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"RequestPhysicalLink : %04x%08x\n", pCall->u.pPhysLink->b.NAP, pCall->u.pPhysLink->b.SAP));
	SVSUTIL_ASSERT (pCall->eWhat == CALL_PHYS_CONNECT);
	SVSUTIL_ASSERT (pCall->eType == CALL_CTX_INTERNAL);
	SVSUTIL_ASSERT (VerifyPhys (pCall->u.pPhysLink));
	SVSUTIL_ASSERT (gpL2CAP->eStage == Connected);

	HANDLE hHCI = gpL2CAP->hHCI;
	if (! hHCI) {
		IFDBG(DebugOut (DEBUG_ERROR, L"RequestPhysicalLink :: hci does not support physical links\n"));
		return ERROR_INTERNAL_ERROR;
	}

	HCI_CreateConnection_In pHCICall = gpL2CAP->hci_if.hci_CreateConnection_In;
	unsigned short usPacketType = pCall->u.pPhysLink->usPacketType ? pCall->u.pPhysLink->usPacketType : gpL2CAP->usPacketType;

	BT_LAYER_IO_CONTROL pIoctl = gpL2CAP->hci_if.hci_ioctl;

	BD_ADDR b = pCall->u.pPhysLink->b;

	gpL2CAP->AddRef ();
	gpL2CAP->Unlock ();

	int iRes = ERROR_INTERNAL_ERROR;

	__try {
		InquiryResultBuffer irb;
		int cSizeRet = 0;
		iRes = pIoctl (hHCI, BTH_HCI_IOCTL_GET_LAST_INQUIRY_DATA, sizeof(b), (char *)&b,
						sizeof(irb), (char *)&irb, &cSizeRet);

		if ((iRes != ERROR_SUCCESS) || (cSizeRet != sizeof(irb)))
			memset (&irb, 0, sizeof(irb));

		iRes = pHCICall (hHCI, pCall, &b, usPacketType, irb.page_scan_repetition_mode, irb.page_scan_mode, irb.clock_offset, 1);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"[L2CAP] RequestPhysicalLink :: Exception in hci_CreateConnection_In\n"));
	}

	gpL2CAP->Lock ();
	gpL2CAP->DelRef ();

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"RequestPhysicalLink : %04x%08x returns %d\n", b.NAP, b.SAP, iRes));

	return iRes;
}

static int MakePhysicalLink (L2CAP_CONTEXT *pOwner, BD_ADDR *pba, PhysLink **ppLink) {
	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakePhysicalLink %04x%08x\n", pba->NAP, pba->SAP));

	if (gpL2CAP->eStage != Connected) {
		IFDBG(DebugOut (DEBUG_ERROR, L"MakePhysicalLink :: ERROR_SERVICE_NOT_ACTIVE\n"));
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	*ppLink = FindPhys (pba);
	if (*ppLink) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakePhysicalLink :: found existing\n"));
		return ERROR_SUCCESS;
	}

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakePhysicalLink :: making new\n"));

	*ppLink = CreateNewPhysLink ();
	if (!  *ppLink) {
		IFDBG(DebugOut (DEBUG_ERROR, L"MakePhysicalLink :: ERROR_OUTOFMEMORY\n"));
		return ERROR_OUTOFMEMORY;
	}

	CallContext *pCall = AllocCallContext (CALL_CTX_INTERNAL, CALL_PHYS_CONNECT, *ppLink, NULL, NULL);
	if (! pCall) {
		delete *ppLink;
		*ppLink = NULL;
		IFDBG(DebugOut (DEBUG_ERROR, L"MakePhysicalLink :: ERROR_OUTOFMEMORY\n"));
		return ERROR_OUTOFMEMORY;
	}

	(*ppLink)->b = *pba;
	(*ppLink)->usPacketType = pOwner->usPacketType;
	(*ppLink)->pNext = gpL2CAP->pPhysLinks;
	gpL2CAP->pPhysLinks = *ppLink;

	int iRes = ERROR_INTERNAL_ERROR;

	if (! gpL2CAP->fPicoCapable) {
		//
		//	Note, this might look funny, but if there's a single existing "live" link - better don't mess with it
		//	and just go through with the connection.
		//
		PhysLink *pPhys = gpL2CAP->pPhysLinks;
		while (pPhys && (pPhys->eStage != UP))
			pPhys = pPhys->pNext;

		if (pPhys && (! pPhys->pLogLinks) && (! FindCall (pPhys))) {	// Timeouting link...
			IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakePhysicalLink :: disconnecting idle connection (to %04x%08x) first\n", pPhys->b.NAP, pPhys->b.SAP));
			iRes = DisconnectPhysicalLink (pPhys, FALSE, ERROR_SUCCESS, pCall);
		} else
			IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakePhysicalLink :: Idle connection not found!\n"));
	} else
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakePhysicalLink :: hardware is piconet-capable!\n"));
	
	if ((gpL2CAP->eStage == Connected) && (iRes != ERROR_SUCCESS) && VerifyCall (pCall))
		iRes = RequestPhysicalLink (pCall);

	if ((iRes != ERROR_SUCCESS) && VerifyPhys (*ppLink)) {
		DisconnectPhysicalLink (*ppLink, FALSE, iRes, NULL);
		*ppLink = NULL;
	}

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakePhysicalLink :: returning %d\n", iRes));

	return iRes;
}

static int SendConnectionRequest (CallContext *pCall, int fAbortCall) {
	SVSUTIL_ASSERT (VerifyLog (pCall->u.pLogLink));
	SVSUTIL_ASSERT (pCall->eWhat == CALL_LOG_CONNECT_REQ);
	SVSUTIL_ASSERT (pCall->eType == CALL_CTX_CALLOWNER);

	LogLink *pLink = pCall->u.pLogLink;

	pLink->cid = GetCID ();
	pLink->eStage = STARTING;

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"Sending connection request for psm 0x%04x (local cid = 0x%04x, id = 0x%02x)\n",
		pLink->psm, pLink->cid, pCall->id));

	if (! gpL2CAP->hHCI) {
		IFDBG(DebugOut (DEBUG_ERROR, L"HCI disconnected!\n"));

		if (fAbortCall)
			DisconnectLogicalLink (pLink, ERROR_INTERNAL_ERROR, FALSE);

		return ERROR_INTERNAL_ERROR;
	}

	Signal s;
	s.cid = 1;
	s.length = sizeof (s.packet.h) + sizeof(s.packet.u.CONNECTION_REQUEST);
	s.packet.h.code = L2CAP_COMMAND_CONNECTION_REQUEST;
	s.packet.h.id = pCall->id;
	s.packet.h.length = sizeof(s.packet.u.CONNECTION_REQUEST);
	s.packet.u.CONNECTION_REQUEST.psm = pLink->psm;
	s.packet.u.CONNECTION_REQUEST.source_cid = pLink->cid;

	int iRes = WriteDataDown (pLink->pPhysLink->h, pCall, SIGNAL_LENGTH(s), (unsigned char *)&s);

	if (fAbortCall && (iRes != ERROR_SUCCESS) && gpL2CAP->IsStackRunning () && (pLink == VerifyLog (pLink)))
		DisconnectLogicalLink (pLink, iRes, FALSE);

	return iRes;
}

static int MakeLogicalLink (L2CAP_CONTEXT *pOwner, void *pCallContext, PhysLink *pPhysLink, unsigned short psm) {
	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakeLogicalLink to %04x%08x psm 0x%04x\n", pPhysLink->b.NAP, pPhysLink->b.SAP, psm));

	LogLink *pLink = CreateNewLogLink (pPhysLink, pOwner);
	if (! pLink) {
		IFDBG(DebugOut (DEBUG_ERROR, L"MakeLogicalLink : ERROR_OUTOFMEMORY\n"));
		return ERROR_OUTOFMEMORY;
	}

	CallContext *pCall = AllocCallContext (CALL_CTX_CALLOWNER, CALL_LOG_CONNECT_REQ, pLink, pOwner, pCallContext);
	if (! pCall) {
		PluckLogLink (pLink);
		IFDBG(DebugOut (DEBUG_ERROR, L"MakeLogicalLink : ERROR_OUTOFMEMORY (no call)\n"));

		return ERROR_OUTOFMEMORY;
	}

	pLink->psm = psm;

	int iRes = ERROR_SUCCESS;
	if (pPhysLink->eStage == UP)
		iRes = SendConnectionRequest (pCall, FALSE);
	else
		pLink->eStage = STARTING_PHYS;

	if (iRes != ERROR_SUCCESS) {
		IFDBG(DebugOut (DEBUG_ERROR, L"MakeLogicalLink failed (%d) - deleting link\n", iRes));
		DeleteCallContext (pCall);

		if (VerifyLog (pLink) && PluckLogLink (pLink))
			delete pLink;
	}

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"MakeLogicalLink returns %d\n", iRes));

	return iRes;
}

static void RegisterTransmissionError (PhysLink *pPhys) {
	++pPhys->iTransmissionProblems;
	if (pPhys->iTransmissionProblems > L2CAP_MAXBAD)
		DisconnectPhysicalLink (pPhys, FALSE, ERROR_INVALID_DATA, NULL);
}

static int CancelCall (CallContext *pCall, int iError) {
	int fAbort = TRUE;
	switch (pCall->eWhat) {
	case CALL_PHYS_CONNECT:			// Close physical connection, too
	case CALL_PHYS_ACCEPT:
		DisconnectPhysicalLink (pCall->u.pPhysLink, FALSE, iError, NULL);
		fAbort = FALSE;
		break;

	case CALL_LOG_CONNECT_REQ:		// Close logical connection, too
	case CALL_LOG_CONNECT_RESP:
		if (pCall->u.pLogLink)
			DisconnectLogicalLink (pCall->u.pLogLink, iError, FALSE);
		fAbort = FALSE;
		break;

	case CALL_LOG_CONFIG_REQ:
	case CALL_LOG_CONFIG_RESP:
		DisconnectLogicalLink (pCall->u.pLogLink, iError, TRUE);
		fAbort = FALSE;
		break;

	case CALL_PHYS_DISCONNECT:
	case CALL_PHYS_DROP_IDLE:
	case CALL_PHYS_PING:
	case CALL_HCI_READSCAN:
	case CALL_HCI_WRITESCAN:
	case CALL_LOG_DISCONNECT_REQ:	// Do nothing, just cancel call
	case CALL_USERDATA:
		break;

	default:
		SVSUTIL_ASSERT (0);
	}

	if (fAbort)
		AbortCall (pCall, iError);

	return ERROR_SUCCESS;
}

//
//	HCI callbacks
//
static int hci_read_scan_enable_out (void *pCallContext, unsigned char status, unsigned char scan_enable) {
	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_read_scan_enable_out: status = %d, scan = %d\n", status, scan_enable));

	if (! gpL2CAP) {
		IFDBG(DebugOut (DEBUG_ERROR, L"hci_read_scan_enable_out: ERROR_SERVICE_NOT_ACTIVE\n"));
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	gpL2CAP->Lock ();

	if ((gpL2CAP->eStage != Connected) && (gpL2CAP->eStage != ShuttingDown)) {
		IFDBG(DebugOut (DEBUG_ERROR, L"hci_read_scan_enable_out: ERROR_SERVICE_NOT_ACTIVE\n"));
		gpL2CAP->Unlock ();
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	CallContext *pCall = VerifyCall ((CallContext *)pCallContext);
	if (! pCall) {
		IFDBG(DebugOut (DEBUG_ERROR, L"hci_read_scan_enable_out: ERROR_NOT_FOUND\n"));
		gpL2CAP->Unlock ();
		return ERROR_NOT_FOUND;
	}

	SVSUTIL_ASSERT (pCall->eType == CALL_CTX_EVENT);
	SVSUTIL_ASSERT (pCall->eWhat == CALL_HCI_READSCAN);
	SVSUTIL_ASSERT (pCall->pOwner == NULL);
	SVSUTIL_ASSERT (pCall->fKeepOnAbort);

	pCall->r.ucresult[0] = scan_enable;

	AbortCall (pCall, StatusToError (status, ERROR_INTERNAL_ERROR));

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_read_scan_enable_out: ERROR_SUCCESS\n"));
	gpL2CAP->Unlock ();
	return ERROR_SUCCESS;
}

static int hci_disconnect_out (void *pCallContext, unsigned char status) {
	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnect_out: status = %d\n", status));

	if (! gpL2CAP) {
		IFDBG(DebugOut (DEBUG_ERROR, L"hci_disconnect_out: ERROR_SERVICE_NOT_ACTIVE\n"));
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	gpL2CAP->Lock ();

	if ((gpL2CAP->eStage != Connected) && (gpL2CAP->eStage != ShuttingDown)) {
		IFDBG(DebugOut (DEBUG_ERROR, L"hci_disconnect_out: ERROR_SERVICE_NOT_ACTIVE\n"));
		gpL2CAP->Unlock ();
		return ERROR_SERVICE_NOT_ACTIVE;
	}

	CallContext *pCall = VerifyCall ((CallContext *)pCallContext);
	if (! pCall) {
		IFDBG(DebugOut (DEBUG_ERROR, L"hci_disconnect_out: ERROR_NOT_FOUND\n"));
		gpL2CAP->Unlock ();
		return ERROR_NOT_FOUND;
	}

	if (status) {
		if (pCall->eWhat == CALL_PHYS_CONNECT) {
			//
			//	Note, this might look funny, but if there's a single existing "live" link - better don't mess with it
			//	and just go through with the connection.
			//
			int iRes = ERROR_INTERNAL_ERROR;

			PhysLink *pPhys = gpL2CAP->pPhysLinks;

			while (pPhys && (pPhys->eStage != UP))
				pPhys = pPhys->pNext;

			if (pPhys && (! pPhys->pLogLinks) && (! FindCall (pPhys))) {	// Timeouting link...
				IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnect_out :: disconnecting idle connection (to %04x%08x) first\n", pPhys->b.NAP, pPhys->b.SAP));
				iRes = DisconnectPhysicalLink (pPhys, FALSE, ERROR_SUCCESS, pCall);
			} else
				IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnect_out :: no idle connection detected, going into connect\n"));

			if ((iRes != ERROR_SUCCESS) && (gpL2CAP->eStage == Connected) && VerifyCall (pCall)) {
				pPhys = pCall->u.pPhysLink;

				SVSUTIL_ASSERT (VerifyPhys (pPhys));

				iRes = RequestPhysicalLink (pCall);

				if ((iRes != ERROR_SUCCESS) && VerifyPhys (pPhys))
					DisconnectPhysicalLink (pPhys, FALSE, iRes, NULL);
			}
		} else if (pCall->eWhat == CALL_PHYS_DROP_IDLE) {
			AbortCall (pCall, StatusToError (status, ERROR_INTERNAL_ERROR));
		} else {
			SVSUTIL_ASSERT (pCall->eType == CALL_CTX_EVENT);
			SVSUTIL_ASSERT (pCall->eWhat == CALL_PHYS_DISCONNECT);
			SVSUTIL_ASSERT (pCall->pOwner == NULL);
			SVSUTIL_ASSERT (pCall->fKeepOnAbort);

			AbortCall (pCall, StatusToError (status, ERROR_INTERNAL_ERROR));
		}
	}

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnect_out: ERROR_SUCCESS\n"));
	gpL2CAP->Unlock ();
	return ERROR_SUCCESS;
}

static int hci_disconnection_complete_event (void *pUserContext, void *pCallContext, unsigned char status, unsigned short connection_handle, unsigned char reason) {
	SVSUTIL_ASSERT (pUserContext == gpL2CAP);

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnection_complete_event: connection 0x%04x status %d\n", connection_handle, status));
	if ((! gpL2CAP) || (pUserContext != gpL2CAP)) {
		IFDBG(DebugOut (DEBUG_ERROR, L"Completely desynchronized\n"));
		return ERROR_INTERNAL_ERROR;
	}

	gpL2CAP->Lock ();

	CallContext *pCall = VerifyCall ((CallContext *)pCallContext);
	if (pCall) {
		if (pCall->eWhat == CALL_PHYS_CONNECT) {
			//
			//	Note, this might look funny, but if there's a single existing "live" link - better don't mess with it
			//	and just go through with the connection.
			//
			int iRes = ERROR_INTERNAL_ERROR;

			PhysLink *pPhys = gpL2CAP->pPhysLinks;

			while (pPhys && (pPhys->eStage != UP))
				pPhys = pPhys->pNext;

			if (pPhys && (! pPhys->pLogLinks) && (! FindCall (pPhys))) {	// Timeouting link...
				IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnection_complete_event :: disconnecting idle connection (to %04x%08x) first\n", pPhys->b.NAP, pPhys->b.SAP));
				iRes = DisconnectPhysicalLink (pPhys, FALSE, ERROR_SUCCESS, pCall);
			} else
				IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnection_complete_event :: no idle connection detected, going into connect\n"));

			if ((iRes != ERROR_SUCCESS) && (gpL2CAP->eStage == Connected) && VerifyCall (pCall)) {
				pPhys = pCall->u.pPhysLink;

				SVSUTIL_ASSERT (VerifyPhys (pPhys));

				iRes = RequestPhysicalLink (pCall);

				if ((iRes != ERROR_SUCCESS) && VerifyPhys (pPhys))
					DisconnectPhysicalLink (pPhys, FALSE, iRes, NULL);
			}
		} else if (pCall->eWhat == CALL_PHYS_DROP_IDLE) {
			IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"hci_disconnection_complete_event :: disconnected idle connection...\n"));

			SVSUTIL_ASSERT (pCall->eType == CALL_CTX_CALLOWNER);
			SVSUTIL_ASSERT (! pCall->fKeepOnAbort);

			L2CAP_CONTEXT *pOwner = pCall->pOwner;
			BT_LAYER_STACK_EVENT_IND pCallback = pOwner->ei.l2ca_StackEvent;
			if (! pCallback) { // eat the response...
				if (pCall)
					AbortCall (pCall, ERROR_CALL_NOT_IMPLEMENTED);

				IFDBG(DebugOut (DEBUG_ERROR, L"[L2CAP] hci_disconnection_complete_event : No callback, or no connection\n"));
			} else {
				void *pCallContext = pCall->pContext;
				DeleteCallContext (pCall);

				//	If we have more than one open 
				IFDBG(DebugOut (DEBUG_L2CAP_CALLBACK, L"Going into l2ca_StackEvent\n"));
				pOwner->AddRef ();
				gpL2CAP->Unlock ();
				__try {
					pCallback (pOwner, BTH_STACK_L2CAP_DROP_COMPLETE, pCallContext);
				} __except (1) {
					IFDBG(DebugOut (DEBUG_ERROR, L"Exception in l2ca_StackEvent!!!\n"));
				}
				gpL2CAP->Lock ();
				pOwner->DelRef ();
				IFDBG(DebugOut (DEBUG_L2CAP_CALLBACK, L"Came from l2ca_StackEvent\n"));
			}
		} else {
			SVSUTIL_ASSERT (pCall->eType == CALL_CTX_EVENT);
			SVSUTIL_ASSERT (pCall->eWhat == CALL_PHYS_DISCONNECT);
			SVSUTIL_ASSERT (pCall->pOwner == NULL);
			SVSUTIL_ASSERT (pCall->fKeepOnAbort);
			SVSUTIL_ASSERT (pCall->u.pPhysLink == FindPhys (connection_handle));

			pCall->fComplete = TRUE;
			pCall->iResult = StatusToError (status, ERROR_INTERNAL_ERROR);
	
			if (pCall->eType == CALL_CTX_EVENT)
				SetEvent (pCall->hEvent);
		}
	}

	if ((status == 0) && (gpL2CAP->eStage == Connected)) {		// Get rid of all logical connections
		PhysLink *pPhys = FindPhys (connection_handle);
		if (pPhys) {
			pPhys->eStage = DISCONNECTED;
			DisconnectPhysicalLink (pPhys, TRUE, ERROR_CONNECTION_UNAVAIL, NULL);
		}
	}