l2cap.cxx

三星2440原版bsp

	return pP;
}

static PhysLink *FindPhys (BD_ADDR *pba) {
	PhysLink *pP = gpL2CAP->pPhysLinks;
	while (pP && (pP->b != *pba))
		pP = pP->pNext;

	return pP;
}

static LogLink *FindLog (unsigned short cid, PhysLink *pPhys) {
	LogLink *pLog = pPhys->pLogLinks;
	while (pLog) {
		if (pLog->cid == cid)
			break;

		pLog = pLog->pNext;
	}

	return pLog;
}

static LogLink *FindLog (unsigned short cid) {
	PhysLink *pPhys = gpL2CAP->pPhysLinks;
	while (pPhys) {
		LogLink *pLog = pPhys->pLogLinks;
		while (pLog) {
			if (pLog->cid == cid)
				return pLog;

			pLog = pLog->pNext;
		}

		pPhys = pPhys->pNext;
	}

	return NULL;
}

static CallContext *FindCall (LogLink *pLog) {
	CallContext *pCall = gpL2CAP->pCalls;
	while (pCall && (pCall->u.pLogLink != pLog))
		pCall = pCall->pNext;

	return pCall;
}

static CallContext *FindCall (LogLink *pLog, CALL_OP eOp) {
	CallContext *pCall = gpL2CAP->pCalls;
	while (pCall && ((pCall->u.pLogLink != pLog) || (pCall->eWhat != eOp)))
		pCall = pCall->pNext;

	return pCall;
}

static CallContext *FindCall (LogLink *pLog, CALL_OP eOp, unsigned char id) {
	CallContext *pCall = gpL2CAP->pCalls;
	while (pCall && ((pCall->u.pLogLink != pLog) || (pCall->eWhat != eOp) || (pCall->id != id)))
		pCall = pCall->pNext;

	return pCall;
}

static CallContext *FindCall (PhysLink *pPhys) {
	CallContext *pCall = gpL2CAP->pCalls;
	while (pCall && (pCall->u.pPhysLink != pPhys))
		pCall = pCall->pNext;

	return pCall;
}

static CallContext *FindCall (PhysLink *pPhys, CALL_OP eOp) {
	CallContext *pCall = gpL2CAP->pCalls;
	while (pCall && ((pCall->u.pPhysLink != pPhys) || (pCall->eWhat != eOp)))
		pCall = pCall->pNext;

	return pCall;
}

static CallContext *FindCall (PhysLink *pPhys, CALL_OP eOp, unsigned char id) {
	CallContext *pCall = gpL2CAP->pCalls;
	while (pCall && ((pCall->u.pPhysLink != pPhys) || (pCall->eWhat != eOp) || (pCall->id != id)))
		pCall = pCall->pNext;

	return pCall;
}

static CallContext *FindCall (PhysLink *pPhys, unsigned char id) {
	CallContext *pCall = gpL2CAP->pCalls;
	while (pCall) {
		if (pCall->id == id) {
			switch (pCall->eWhat) {
			case CALL_HCI_READSCAN:
			case CALL_HCI_WRITESCAN:
			case CALL_PHYS_DROP_IDLE:		// This does NOT have physlink in it
			case CALL_PHYS_DISCONNECT:		// This does NOT have physlink in it
				break;

			case CALL_PHYS_CONNECT:
			case CALL_PHYS_ACCEPT:
			case CALL_PHYS_PING:
				if (pPhys == pCall->u.pPhysLink)
					return pCall;
				break;

			case CALL_LOG_CONNECT_RESP:
			case CALL_LOG_CONFIG_RESP:
				if (pCall->fForeignId)
					break;

			case CALL_LOG_CONNECT_REQ:
			case CALL_LOG_CONFIG_REQ:
			case CALL_USERDATA:
			case CALL_LOG_DISCONNECT_REQ:
				if (pPhys == pCall->u.pLogLink->pPhysLink)
					return pCall;
				break;

			default:
				SVSUTIL_ASSERT (0);
			}
		}
		pCall = pCall->pNext;
	}

	return NULL;
}

static CallContext *FindCall (void *pUserContext) {
	CallContext *pCall = gpL2CAP->pCalls;

	while (pCall && (pCall->pContext != pUserContext))
		pCall = pCall->pNext;

	return pCall;
}

static LogLink *FindCID (unsigned short cid, PhysLink *pHint) {
	PhysLink *pP = pHint ? pHint : gpL2CAP->pPhysLinks;
	while (pP) {
		LogLink *pL = pP->pLogLinks;
		while (pL) {
			if (pL->cid == cid)
				return pL;
			pL = pL->pNext;
		}

		pP = pHint ? NULL : pP->pNext;
	}

	return NULL;
}

static L2CAP_CONTEXT *FindContextByPSM (unsigned short psm) {
	L2CAP_CONTEXT *pCtx = gpL2CAP->pContexts;
	while (pCtx) {
		PSMContext *pPSMCtx = pCtx->pReservedPorts;
		while (pPSMCtx) {
			if (pPSMCtx->usPSM == psm)
				return pCtx;
			pPSMCtx = pPSMCtx->pNext;
		}

		pCtx = pCtx->pNext;
	}

	return NULL;
}

static PhysLink *VerifyPhys (PhysLink *pLink) {
	PhysLink *pP = gpL2CAP->pPhysLinks;
	while (pP && (pP != pLink))
		pP = pP->pNext;

	return pP;
}

static LogLink *VerifyLog (LogLink *pLink) {
	PhysLink *pP = VerifyPhys (pLink->pPhysLink);
	if (! pP)
		return NULL;

	LogLink *pL = pP->pLogLinks;

	while (pL && (pL != pLink))
		pL = pL->pNext;

	return pL;
}

static L2CAP_CONTEXT *VerifyContext (L2CAP_CONTEXT *pContext) {
	L2CAP_CONTEXT *pRunner = gpL2CAP->pContexts;
	while (pRunner && (pRunner != pContext))
		pRunner = pRunner->pNext;

	return pRunner;
}

static CallContext *VerifyCall (CallContext *pCallContext) {
	CallContext *pContext = gpL2CAP->pCalls;
	while (pContext && (pContext != pCallContext))
		pContext = pContext->pNext;
	return pContext;
}

static PhysLink *CreateNewPhysLink (void) {
	return new PhysLink;
}

static LogLink *CreateNewLogLink (PhysLink *pPhys, L2CAP_CONTEXT *pOwner) {
	return new LogLink (pPhys, pOwner);
}

static void GetConnectionState (void) {
	__try {
		int fConnected = FALSE;
		int dwRet = 0;
		gpL2CAP->hci_if.hci_ioctl (gpL2CAP->hHCI, BTH_STACK_IOCTL_GET_CONNECTED, 0, NULL, sizeof(fConnected), (char *)&fConnected, &dwRet);
		if ((dwRet == sizeof(fConnected)) && fConnected)
			gpL2CAP->eStage = Connected;
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"[L2CAP] GetConnectionState : exception in hci_ioctl BTH_STACK_IOCTL_GET_CONNECTED\n"));
	}
}

static void SetScanEnable (void) {
	if (! gpL2CAP->fScanModeControl)
		return;

	CallContext *pCall = AllocCallContext (CALL_CTX_EVENT, CALL_HCI_READSCAN, NULL, NULL, NULL);
	pCall->fKeepOnAbort = TRUE;

	HCI_ReadScanEnable_In pCallback = gpL2CAP->hci_if.hci_ReadScanEnable_In;
	HANDLE hHCI = gpL2CAP->hHCI;
	HANDLE hEvent = pCall->hEvent;
	gpL2CAP->AddRef ();
	gpL2CAP->Unlock ();
	int iRes = ERROR_INTERNAL_ERROR;

	__try {
		iRes = pCallback (hHCI, pCall);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"[L2CAP] SetScanEnable : EXCEPTION in hci_ReadScanEnable_In\n"));
	}

	if (iRes == ERROR_SUCCESS)
		WaitForSingleObject (hEvent, L2CAP_TO);

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

	if ((! (pCall = VerifyCall (pCall))) || (! pCall->fComplete) || (iRes != ERROR_SUCCESS) || (pCall->iResult != ERROR_SUCCESS)) {
		if (pCall && gpL2CAP->IsStackRunning ())
			DeleteCallContext (pCall);

		IFDBG(DebugOut (DEBUG_ERROR, L"[L2CAP] SetScanEnable failed read. \n"));
		return;
	}

	unsigned char cScanEnable = pCall->r.ucresult[0];
	DeleteCallContext (pCall);

	if ((cScanEnable & 0x3) != 0x3) {
		HCI_WriteScanEnable_In pCallback = gpL2CAP->hci_if.hci_WriteScanEnable_In;
		gpL2CAP->AddRef ();
		gpL2CAP->Unlock ();
		pCallback (hHCI, NULL, cScanEnable | 0x3);
		gpL2CAP->Lock ();
		gpL2CAP->DelRef ();
	}
}

static void WriteLinkPolicy (unsigned short hconnection) {
	if (gpL2CAP->usLinkPolicy == 0xffff)
		return;

	HCI_WriteLinkPolicySettings_In pCallback = gpL2CAP->hci_if.hci_WriteLinkPolicySettings_In;
	HANDLE hHCI = gpL2CAP->hHCI;

	gpL2CAP->AddRef ();
	gpL2CAP->Unlock ();
	int iRes = ERROR_INTERNAL_ERROR;

	__try {
		pCallback (hHCI, NULL, hconnection, gpL2CAP->usLinkPolicy);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"[L2CAP] WriteLinkPolicy : EXCEPTION in hci_WriteLinkPolicySettings_In\n"));
	}

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

static int WriteDataDown (unsigned short h, CallContext *pCall, BD_BUFFER *pB) {
	int iRes = ERROR_INTERNAL_ERROR;
	if (gpL2CAP->hHCI) {
		IFDBG(DebugOut (DEBUG_L2CAP_CALLBACK, L"WriteDataDown in\n"));
		HANDLE hHCI = gpL2CAP->hHCI;
		HCI_DataPacketDown_In pCallback = gpL2CAP->hci_if.hci_DataPacketDown_In;

		gpL2CAP->AddRef ();
		gpL2CAP->Unlock ();
		__try {
			iRes = pCallback (hHCI, pCall, h, pB);
		} __except (1) {
			IFDBG(DebugOut (DEBUG_ERROR, L"Exception in WriteDataDown\n"));
		}
		gpL2CAP->Lock ();
		gpL2CAP->DelRef ();
		IFDBG(DebugOut (DEBUG_L2CAP_CALLBACK, L"WriteDataDown out\n"));
	} else
		IFDBG(DebugOut (DEBUG_ERROR, L"HCI layer does not support sending data packets!\n"));

	return iRes;
}

static int WriteDataDown (unsigned short h, CallContext *pCall, int cSize, unsigned char *pBuffer) {
	if ((gpL2CAP->cHCIHeader == 0) && (gpL2CAP->cHCITrailer == 0)) {
		BD_BUFFER b;
		b.cSize = cSize;
		b.cEnd = b.cSize;
		b.cStart = 0;
		b.fMustCopy = TRUE;
		b.pFree = BufferFree;
		b.pBuffer = pBuffer;

		return WriteDataDown (h, pCall, &b);
	}

	BD_BUFFER *pB = BufferAlloc (cSize + gpL2CAP->cHCIHeader + gpL2CAP->cHCITrailer);

	if (pB) {
		pB->cEnd = pB->cSize - gpL2CAP->cHCITrailer;
		pB->cStart = gpL2CAP->cHCIHeader;
		memcpy (pB->pBuffer, pBuffer, cSize);
		return WriteDataDown (h, pCall, pB);
	}

	IFDBG(DebugOut (DEBUG_ERROR, L"OOM in WriteDataDown!\n"));
	return ERROR_OUTOFMEMORY;
}

static unsigned short GetCID (void) {
	for ( ; ; ) {
		unsigned short cid = ++gpL2CAP->usCurrentCID;
		if (cid < L2CAP_FIRST_CID) {
			gpL2CAP->usCurrentCID = L2CAP_FIRST_CID;
			continue;
		}

		if (! FindCID(cid, NULL))
			return cid;
	}
}

static int PluckPhysLink (PhysLink *pLink) {
	if (pLink == gpL2CAP->pPhysLinks)
		gpL2CAP->pPhysLinks = gpL2CAP->pPhysLinks->pNext;
	else {
		PhysLink *pParent = gpL2CAP->pPhysLinks;
		while (pParent && (pParent->pNext != pLink))
			pParent = pParent->pNext;

		if (! pParent)
			return FALSE;

		pParent->pNext = pLink->pNext;
	}

	return TRUE;
}

static int PluckLogLink (LogLink *pLink) {
	//	Take it out of the list
	if (pLink == pLink->pPhysLink->pLogLinks)
		pLink->pPhysLink->pLogLinks = pLink->pNext;
	else {
		LogLink *pParent = pLink->pPhysLink->pLogLinks;
		while (pParent && (pParent->pNext != pLink))
			pParent = pParent->pNext;

		if (! pParent)
			return FALSE;

		pParent->pNext = pLink->pNext;
	}

	return TRUE;
}
//
//	Timer
//
static DWORD WINAPI PhysLinkTimeout (LPVOID pArg) {
	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"PhysLinkTimeout\n"));

	if (! gpL2CAP) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"PhysLinkTimeout : shutting down!\n"));
		return 0;
	}

	gpL2CAP->Lock ();

	if (gpL2CAP->eStage != Connected) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"PhysLinkTimeout : shutting down!\n"));
		gpL2CAP->Unlock ();
		return 0;
	}

	PhysLink *pLink = VerifyPhys ((PhysLink *)pArg);
	if (! pLink) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"PhysLinkTimeout : no link!\n"));
		gpL2CAP->Unlock ();
		return 0;
	}

	pLink->dwTimeOutCookie = 0;

	if (! (pLink->iPingsSent || pLink->pLogLinks || pLink->iLockCnt)) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"PhysLinkTimeout : timeouting the connection\n"));
		DisconnectPhysicalLink (pLink, FALSE, ERROR_SUCCESS, NULL);
	}

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"PhysLinkTimeout : done\n"));
	gpL2CAP->Unlock ();
	return 0;
}

static DWORD WINAPI LogLinkTimeout (LPVOID pArg) {
	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"LogLinkTimeout\n"));

	if (! gpL2CAP) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"LogLinkTimeout : shutting down!\n"));
		return 0;
	}

	gpL2CAP->Lock ();

	if (gpL2CAP->eStage != Connected) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"LogLinkTimeout : shutting down!\n"));
		gpL2CAP->Unlock ();
		return 0;
	}

	LogLink *pLink = VerifyLog ((LogLink *)pArg);
	if (! pLink) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"LogLinkTimeout : no link!\n"));
		gpL2CAP->Unlock ();
		return 0;
	}

	if ((pLink->dwTimeOutCookie != 0) && (pLink->eStage != UP)) {
		IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"LogLinkTimeout : timeouting the connection\n"));

		pLink->dwTimeOutCookie = 0;

		DisconnectLogicalLink (pLink, ERROR_TIMEOUT, TRUE);
	}

	IFDBG(DebugOut (DEBUG_L2CAP_TRACE, L"LogLinkTimeout : done\n"));
	gpL2CAP->Unlock ();
	return 0;
}