rfcomm.cxx

三星2440原版bsp

		}

		if (gpRFCOMM->GetRefCount () == 1)
			break;

		IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"Waiting for ref count in StackUp\n"));
		gpRFCOMM->Unlock ();
		Sleep (100);
	}

	GetConnectionState ();

	if (gpRFCOMM->fConnected) {
		gpRFCOMM->AddRef ();
		DispatchStackEvent (BTH_STACK_UP);
		gpRFCOMM->DelRef ();
	}

	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"-StackUp\n"));
	gpRFCOMM->Unlock ();

	return ERROR_SUCCESS;
}

static DWORD WINAPI StackReset (LPVOID pArg) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"Reset stack\n"));

	StackDown (NULL);
	StackUp (NULL);

	return NULL;
}

static DWORD WINAPI StackDisconnect (LPVOID pArg) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"Connect stack\n"));

	rfcomm_CloseDriverInstance ();

	return NULL;
}


static int SendConfigRequest (Session *pSess) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"SendConfigRequest %04x%08x\n", pSess->b.NAP, pSess->b.SAP));

	Task *pTask = NewTask (CALL_L2CAP_LINK_CONFIG, NULL, pSess);

	AddTask (pTask);

	HANDLE h = gpRFCOMM->hL2CAP;
	L2CA_ConfigReq_In pCallback = gpRFCOMM->l2cap_if.l2ca_ConfigReq_In;
	unsigned short cid = pSess->cid;

	gpRFCOMM->AddRef ();
	gpRFCOMM->Unlock ();

	IFDBG(DebugOut (DEBUG_RFCOMM_CALLBACK, L"SendConfigRequest : going into l2ca_ConfigReq_In\n"));

	int iRes = ERROR_INTERNAL_ERROR;
	__try {
		iRes = pCallback (h, pTask, cid, pSess->inMTU, 0xffff, NULL, 0, NULL);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"RFCOMM :: Exception in l2ca_ConfigReq_In!\n"));
	}

	gpRFCOMM->Lock ();
	gpRFCOMM->DelRef ();

	IFDBG(DebugOut (DEBUG_RFCOMM_CALLBACK, L"SendConfigRequest : came out of l2ca_ConfigReq_In\n"));

	if ((iRes != ERROR_SUCCESS) && gpRFCOMM->fRunning && VerifyLink (pSess))
		CloseSession (pSess, iRes, TRUE);

	return iRes;
}

static void SessionIsUp (Session *pSess) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"SessionIsUp %04x%08x\n", pSess->b.NAP, pSess->b.SAP));

	SVSUTIL_ASSERT (gpRFCOMM->fConnected);
	SVSUTIL_ASSERT (pSess->fStage == L2CAPUP);

	// Start channels. Start redirector.
	if (! pSess->fIncoming) {
		Task *pSendSABM0 = NewTask (CALL_RFCOMM_SABM0, NULL, pSess);
		if (pSendSABM0) {
			SVSUTIL_ASSERT (pSendSABM0->channel == 0);

			AddTask (pSendSABM0);

			SVSUTIL_ASSERT (! pSess->fBusy);
			SVSUTIL_ASSERT (! pSess->fWaitAck);

			SetBusy (pSess, pSendSABM0);

			int iRes = SendFrame (pSendSABM0, pSess->cid, 0, pSess->fIncoming, CTL_SABM);

			if ((iRes != ERROR_SUCCESS) && gpRFCOMM->fRunning)
				CloseSession (pSess, iRes, TRUE);
		} else
			CloseSession (pSess, ERROR_OUTOFMEMORY, TRUE);
	} else if (! pSess->pLogLinks)
		ScheduleTimeout (pSess, RFCOMM_T1);
}

static Task *GetTaskByAddress (Session *pSess, unsigned char ucAddress) {
	unsigned char channel = ucAddress >> 3;
	unsigned char fLocal  = IsLocal (pSess, ucAddress >> 2);

	Task *pT = gpRFCOMM->pCalls;
	while (pT) {
		if ((pT->fWhat == CALL_RFCOMM_SIGNALDATA) && (pT->pPhysLink == pSess) && (pT->channel == channel) && (pT->fLocal == fLocal))
			break;

		pT = pT->pNext;
	}

	return pT;
}

static Task *GetTaskByResponse (Session *pSess, unsigned char cType, unsigned char ucAddress) {
	unsigned char channel = ucAddress >> 3;
	unsigned char fLocal  = IsLocal (pSess, ucAddress >> 2);

	Task *pT = gpRFCOMM->pCalls;
	while (pT) {
		if ((pT->fWhat == CALL_RFCOMM_SIGNALDATA) && (pT->pPhysLink == pSess) && (pT->eType == cType) && (pT->channel == channel) && (pT->fLocal == fLocal))
			break;

		pT = pT->pNext;
	}

	return pT;
}

static Task *GetTaskByResponse (Session *pSess, unsigned char cType) {
	Task *pT = gpRFCOMM->pCalls;
	while (pT) {
		if ((pT->fWhat == CALL_RFCOMM_SIGNALDATA) && (pT->pPhysLink == pSess) && (pT->eType == cType))
			break;

		pT = pT->pNext;
	}

	return pT;
}

static DLCI *DLCIFromAddress (Session *pSess, unsigned char ucAddress) {
	unsigned char channel = ucAddress >> 3;
	unsigned char fLocal  = IsLocal (pSess, ucAddress >> 2);

	DLCI *pChann = pSess->pLogLinks;

	while (pChann && ((pChann->channel != channel) || (pChann->fLocal != fLocal)))
		pChann = pChann->pNext;

	return pChann;
}

#define BAUD_ARRAY_SIZE	9
static int byte_to_baud[BAUD_ARRAY_SIZE] = {CBR_2400, CBR_4800, 7200, CBR_9600, CBR_19200, CBR_38400, CBR_57600, CBR_115200, 230400};

static int ByteToBaud (unsigned char b) {
	if (BAUD_ARRAY_SIZE <= b) {
		IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ByteToBaud : Unknown baud rate byte, byte=%d!\n", b));
		return -1;
	}

	return byte_to_baud[b];
}

static int BaudToByte (int baud) {
	for (int i = 0 ; i < BAUD_ARRAY_SIZE ; ++i) {
		if (byte_to_baud[i] == baud)
			return i;
	}

	IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ByteToBaud : baud rate translation failed, baud = %d!\n", baud));
	return -1;
}

static int ByteToData(unsigned char b) {
	SVSUTIL_ASSERT (b < 4);
	return 5 + b;
}

static int DataToByte (unsigned char data) {
#if defined (DEBUG) || defined (_DEBUG)
	if ((data < 5) || (data > 8))
		DebugOut (DEBUG_WARN, L"[RFCOMM] DataToByte : incorrect data = %d!\n", data);
#endif

	return (data - 5) & 3;
}

static int ByteToPT (unsigned char b) {
	SVSUTIL_ASSERT (b < 4);
	return b + 1;
}

static int PTToByte (int parity) {
#if defined (DEBUG) || defined (_DEBUG)
	if (parity > 4)
		DebugOut (DEBUG_WARN, L"[RFCOMM] DataToByte : incorrect parity = %d!\n", parity);
#endif

	return (parity - 1) & 3;
}

static void ProcessSignallingData (Session *pSess, BD_BUFFER *pBuff) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[RFCOMM] +ProcessSignallingData session with %04x%08x\n", pSess->b.NAP, pSess->b.SAP));

	int fError = FALSE;
	unsigned char cTypeByte = 0;
	unsigned char cType = 0;
	unsigned char cLen  = 0;

	fError = ! BufferGetByte (pBuff, &cTypeByte);

#if defined (DEBUG) || defined (_DEBUG) || defined (RETAILLOG)
	if (fError)
		IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : empty!\n"));
#endif

	if ((! fError) && (! (cTypeByte & EA_BIT))) {
		IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : command 0x%02x no EA_BIT!\n", cTypeByte));
		fError = TRUE;
	} else
		cType = cTypeByte >> 2;

	if ((! fError) && (! (fError = ! BufferGetByte (pBuff, &cLen)))) {
		if (! (cLen & EA_BIT)) {
			IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : command 0x%02x too long!\n", cTypeByte));
			fError = TRUE;
		} else if ((cLen = cLen >> 1) != BufferTotal (pBuff)) {
			IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : command 0x%02x length mismatch!\n", cTypeByte));
			fError = TRUE;
		}
	}

	if ((! fError) && (cTypeByte & CR_BIT)) {	// Command
		switch (cType) {
		case MSG_TEST:
			{
				IFDBG(DebugOut (DEBUG_RFCOMM_PACKETS, L"ProcessSignallingData : MSG_TEST command arrived (handling)\n"));
				pBuff->cStart -= 2;
				pBuff->pBuffer[pBuff->cStart] &= (~ CR_BIT);	// Response
				SendFrame (NULL, pSess->cid, pSess->fIncoming, BufferTotal(pBuff), pBuff->pBuffer + pBuff->cStart);
				break;
			}

		case MSG_FCON:
		case MSG_FCOFF:
			{
				IFDBG(DebugOut (DEBUG_RFCOMM_PACKETS, L"ProcessSignallingData : MSG_FCON/FCOFF command arrived...\n"));

				if (cLen != 0) {
					IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : MSG_FCON/FCOFF command : incorrect length %d (need 0)...\n", cLen));
					fError = TRUE;
					break;
				}

				unsigned char body[2];

				body[0] = cTypeByte & (~CR_BIT);
				body[1] = 0 | EA_BIT;

				IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"ProcessSignallingData/MSG_FCON/FCOFF : sending response\n"));
				SendFrame (NULL, pSess->cid, pSess->fIncoming, sizeof(body), body);

				while (gpRFCOMM->fConnected && (pSess == VerifyLink (pSess))) {
					DLCI *pDLCI = pSess->pLogLinks;
					while (pDLCI && pDLCI->fFCA)
						pDLCI = pDLCI->pNext;

					if (! pDLCI)
						break;

					IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"ProcessSignallingData/MSG_FCON/FCOFF : notifying connection 0x%08x\n", pDLCI));

					pDLCI->fFCA = 1;
					RFCOMM_CONTEXT *pOwner = pDLCI->pOwner;
					RFCOMM_FC_Ind pCallback = pOwner->ei.rfcomm_FC_Ind;
					void *pUserContext = pOwner->pUserContext;
					pOwner->AddRef ();
					gpRFCOMM->Unlock ();

					__try {
						pCallback (pUserContext, pDLCI, (cType == MSG_FCON) ? TRUE : FALSE);
					} __except (1) {
						IFDBG(DebugOut (DEBUG_ERROR, L"[RFCOMM] ProcessSignallingData : exception in rfcomm_FC_Ind\n"));
					}

					gpRFCOMM->Lock ();
					pOwner->DelRef ();
				}

				if (gpRFCOMM->fConnected && (pSess == VerifyLink (pSess))) {
					DLCI *pDLCI = pSess->pLogLinks;
					while (pDLCI) {
						pDLCI->fFCA = 0;
						pDLCI = pDLCI->pNext;
					}
				}
				break;
			}

		case MSG_MSC:
			{
				IFDBG(DebugOut (DEBUG_RFCOMM_PACKETS, L"ProcessSignallingData : MSG_MSC command arrived...\n"));

				if ((cLen != 2) && (cLen != 3)) {
					IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : MSG_MSC command : incorrect length %d (need 2 or 3)...\n", cLen));
					fError = TRUE;
					break;
				}

				unsigned char body[5];

				body[0] = cTypeByte & (~CR_BIT);
				body[1] = (cLen << 1) | EA_BIT;
				BufferGetChunk (pBuff, cLen, body + 2);

				IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"ProcessSignallingData/MSG_MSC : sending response\n"));
				SendFrame (NULL, pSess->cid, pSess->fIncoming, cLen + 2, body);

				DLCI *pDLCI = NULL;

				if (gpRFCOMM->fConnected && (pSess == VerifyLink (pSess)) && (pDLCI = DLCIFromAddress (pSess, body[2]))) {
					RFCOMM_CONTEXT *pOwner = pDLCI->pOwner;
					RFCOMM_MSC_Ind pCallback = pOwner->ei.rfcomm_MSC_Ind;
					void *pUserContext = pOwner->pUserContext;
					pOwner->AddRef ();
					gpRFCOMM->Unlock ();

					__try {
						pCallback (pUserContext, pDLCI, body[3], cLen == 3 ? body[4] : 0xff);
					} __except (1) {
						IFDBG(DebugOut (DEBUG_ERROR, L"[RFCOMM] ProcessSignallingData : exception in RFCOMM_MSC_Ind\n"));
					}

					gpRFCOMM->Lock ();
					pOwner->DelRef ();
				} else
					IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : Disconnected or channel not found\n"));

				break;
			}

		case MSG_RLS:
			{
				IFDBG(DebugOut (DEBUG_RFCOMM_PACKETS, L"ProcessSignallingData : MSG_RLS command arrived...\n"));

				if (cLen != 2) {
					IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : MSG_RLS command : incorrect length %d (need 2)...\n", cLen));
					fError = TRUE;
					break;
				}

				unsigned char body[4];

				body[0] = cTypeByte & (~CR_BIT);
				body[1] = (2 << 1) | EA_BIT;
				BufferGetChunk (pBuff, 2, body + 2);

				IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"ProcessSignallingData/MSG_RLS : sending response\n"));
				SendFrame (NULL, pSess->cid, pSess->fIncoming, sizeof(body), body);

				DLCI *pDLCI = NULL;

				if (gpRFCOMM->fConnected && (pSess == VerifyLink (pSess)) && (pDLCI = DLCIFromAddress (pSess, body[2]))) {
					RFCOMM_CONTEXT *pOwner = pDLCI->pOwner;
					RFCOMM_RLS_Ind pCallback = pOwner->ei.rfcomm_RLS_Ind;
					void *pUserContext = pOwner->pUserContext;
					pOwner->AddRef ();
					gpRFCOMM->Unlock ();

					__try {
						pCallback (pUserContext, pDLCI, body[3]);
					} __except (1) {
						IFDBG(DebugOut (DEBUG_ERROR, L"[RFCOMM] ProcessSignallingData : exception in RFCOMM_RLS_Ind\n"));
					}

					gpRFCOMM->Lock ();
					pOwner->DelRef ();
				} else
					IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : Disconnected or channel not found\n"));

				break;
			}


		case MSG_PN:
			{
				IFDBG(DebugOut (DEBUG_RFCOMM_PACKETS, L"ProcessSignallingData : MSG_PN command arrived...\n"));

				if (cLen != 8) {
					IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] ProcessSignallingData : MSG_PN command : incorrect length %d (need 8)...\n", cLen));
					fError = TRUE;
					break;
				}

				unsigned char body[8];

				BufferGetChunk (pBuff, 8, body);

				DLCI *pDLCI = DLCIFromAddress (pSess, body[0] << 2);	// Cheat, convert to address...

				if (! pDLCI) {
					if (! IsLocal (pSess, body[0])) {
						IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] trying to PN non-local channel (0x%02x) session %04x%08x\n", body[0], pSess->b.NAP, pSess->b.SAP));
						fError = TRUE;
						break;
					}

					unsigned char channel = body[0] >> 1;

					RFCOMM_CONTEXT *pOwner = FindContextByChannel (channel);

					if (! pOwner)
						pOwner = FindContextForDefault ();
	
					if (! pOwner) {
						IFDBG(DebugOut (DEBUG_WARN, L"[RFCOMM] No owner for PN request!\n"));
						fError = TRUE;
						break;
					}

					pDLCI = NewLog (pSess, channel, TRUE, pOwner);

					if (pDLCI) {
						pDLCI->pNext = pSess->pLogLinks;
						pSess->pLogLinks = pDLCI;
					} else {
						IFDBG(DebugOut (DEBUG_ERROR, L"[RFCOMM] ERROR_OUTOFMEMORY in PN processing (channel creation!)\n"));
						fError = TRUE;
						break;
					}