portemu.cxx

三星2440原版bsp

}

static void DeleteCall (ECall *pCall) {
	ECall *pC = pCall->pContext->pCalls;
	ECall *pP = NULL;

	while (pC && pC != pCall) {
		pP = pC;
		pC = pC->pNext;
	}

	PREFAST_ASSERT (pC);

	if (pP)
		pP->pNext = pC->pNext;
	else
		pCall->pContext->pCalls = pC->pNext;

	delete pCall;
}

static void InsertIO (PORTEMU_CONTEXT *pContext, PendingIO *pio) {
	pio->pNext = NULL;
	if (! pContext->pops)
		pContext->pops = pio;
	else {
		PendingIO *pP = pContext->pops;
		while (pP->pNext)
			pP = pP->pNext;
		pP->pNext = pio;
	}
}

static void RemoveIO (PORTEMU_CONTEXT *pContext, PendingIO *pio) {
	if (GetContext ((DWORD)pContext) != pContext)
		return;

	if (pContext->pops == pio)
		pContext->pops = pio->pNext;
	else {
		PendingIO *pP = pContext->pops;
		while (pP && (pP->pNext != pio))
			pP = pP->pNext;

		if (pP)
			pP->pNext = pio->pNext;
	}

	pio->pNext = NULL;
}

static void ReinitNewConnection (PORTEMU_CONTEXT *pContext, HANDLE hConnection) {
	pContext->iRecvQuotaUsed   = pContext->iSendQuotaUsed = 0;

	memset (pContext->adwOccuredEvents, 0, sizeof(pContext->adwOccuredEvents));
	memset (pContext->adwErr, 0, sizeof(pContext->adwErr));
	pContext->dwModemStatusIn  = MS_RLSD_ON;
	pContext->ucv24out         = MSC_DV_BIT | MSC_RTC_BIT | MSC_RTR_BIT;
	pContext->fc               = 0;
	pContext->fSentXon         = FALSE;

	pContext->fSending         = FALSE;
	pContext->fConnected       = TRUE;
	
	pContext->hConnection      = hConnection;

	SVSUTIL_ASSERT (pContext->sdpCid == 0);
	SVSUTIL_ASSERT (pContext->fSdpQueryOn == 0);
	SVSUTIL_ASSERT (pContext->pbl == NULL);
}

static void ReinitClosedConnection (PORTEMU_CONTEXT *pContext) {
	pContext->fSentXon = FALSE;

	pContext->hConnection    = NULL;
	pContext->iRecvQuotaUsed = 0;
	pContext->iSendQuotaUsed = 0;

	pContext->dwModemStatusIn  = 0;
	pContext->ucv24out         = 0;
	pContext->fc               = 0;

	pContext->credit_fc        = 0;
	pContext->iGaveCredits     = 0;
	pContext->iHaveCredits     = 0;

	pContext->fSending         = FALSE;
	pContext->fConnected       = FALSE;
}

static void CheckCommEvent (PORTEMU_CONTEXT *pContext) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"CheckCommEvent : ctx 0x%08x\n", pContext));

	PendingIO *pParent = NULL;
	PendingIO *pIO = pContext->pops;

	while (pIO) {
		SVSUTIL_ASSERT ((pIO->iPortNum >= 0) && (pIO->iPortNum < PORTEMU_OPEN_MAX));

		DWORD dw;

		if ((pIO->op == COMM_WAIT_EVENT) &&
							(dw = pContext->adwEnabledEvents[pIO->iPortNum] & pContext->adwOccuredEvents[pIO->iPortNum])) {
			IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"CheckCommEvent : ctx 0x%08x found and executed waiter (port %d)...\n", pContext, pIO->iPortNum));

			PendingIO *pNext = pIO->pNext;

			if (pParent)
				pParent->pNext = pIO->pNext;
			else
				pContext->pops = pIO->pNext;

			pIO->pNext       = NULL;
			pIO->dwEvent     = dw;

			if (dw & EV_ERR) {
				pIO->dwLineError = pContext->adwErr[pIO->iPortNum];
				pContext->adwErr[pIO->iPortNum] = 0;
			}
			
			pIO->iIoResult = ERROR_SUCCESS;
			SetEvent (pIO->hEvent);

			pContext->adwOccuredEvents[pIO->iPortNum] = 0;

			pIO = pNext;
			continue;
		}

		pParent = pIO;
		pIO = pIO->pNext;
	}
}

static void RegisterCommEvent (PORTEMU_CONTEXT *pContext, DWORD e, DWORD err) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"Event 0x%04x (%s %s %s %s %s %s %s %s %s %s %s %s) %d: registering\n",
		e,
		(e & EV_RXCHAR ? L"EV_RXCHAR" : L""),
		(e & EV_RXFLAG ? L"EV_RXFLAG" : L""),
		(e & EV_TXEMPTY ? L"EV_TXEMPTY" : L""),
		(e & EV_CTS ? L"EV_CTS" : L""),
		(e & EV_DSR ? L"EV_DSR" : L""),
		(e & EV_RLSD ? L"EV_RLSD" : L""),
		(e & EV_BREAK ? L"EV_BREAK" : L""),
		(e & EV_ERR ? L"EV_ERR" : L""),
		(e & EV_RING ? L"EV_RING" : L""),
		(e & EV_RX80FULL ? L"EV_RX80FULL" : L""),
		(e & EV_EVENT1 ? L"EV_EVENT1" : L""),
		(e & EV_EVENT2 ? L"EV_EVENT2" : L""),
		err));

	for (int i = 0 ; i < PORTEMU_OPEN_MAX ; ++i) {
		pContext->adwOccuredEvents[i] = e;
		if (e == EV_ERR)
			pContext->adwErr[i] = err;
	}

	CheckCommEvent (pContext);
}

static void SetCommEventMask (PORTEMU_CONTEXT *pContext, DWORD dwMask, int iPortNum) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"SetCommEventMask Port %d 0x%04x (%s %s %s %s %s %s %s %s %s %s %s %s)\n",
		iPortNum,
		dwMask,
		(dwMask & EV_RXCHAR ? L"EV_RXCHAR" : L""),
		(dwMask & EV_RXFLAG ? L"EV_RXFLAG" : L""),
		(dwMask & EV_TXEMPTY ? L"EV_TXEMPTY" : L""),
		(dwMask & EV_CTS ? L"EV_CTS" : L""),
		(dwMask & EV_DSR ? L"EV_DSR" : L""),
		(dwMask & EV_RLSD ? L"EV_RLSD" : L""),
		(dwMask & EV_BREAK ? L"EV_BREAK" : L""),
		(dwMask & EV_ERR ? L"EV_ERR" : L""),
		(dwMask & EV_RING ? L"EV_RING" : L""),
		(dwMask & EV_RX80FULL ? L"EV_RX80FULL" : L""),
		(dwMask & EV_EVENT1 ? L"EV_EVENT1" : L""),
		(dwMask & EV_EVENT2 ? L"EV_EVENT2" : L"")));

	pContext->adwEnabledEvents[iPortNum] = dwMask;
	pContext->adwOccuredEvents[iPortNum] = 0;

	PendingIO *pParent = NULL;
	PendingIO *pIO = pContext->pops;

	while (pIO) {
		if ((pIO->op == COMM_WAIT_EVENT) && (pIO->iPortNum == iPortNum)) {
			PendingIO *pNext = pIO->pNext;

			pIO->pNext     = NULL;
			pIO->dwEvent   = 0;
			pIO->iIoResult = ERROR_SUCCESS;

			SetEvent (pIO->hEvent);
			
			if (pParent)
				pParent->pNext = pNext;
			else
				pContext->pops = pNext;
			pIO = pNext;
		} else {
			pParent = pIO;
			pIO = pIO->pNext;
		}
	}
 }

static void RFCommDisconnectIn (PORTEMU_CONTEXT *pContext) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] RFCommDisconnect 0x%08x (handle 0x%08x device %04x%08x ch 0x%02x\n",
					pContext, pContext->hConnection, pContext->b.NAP, pContext->b.SAP, pContext->channel));

	HANDLE hConn = pContext->hConnection;

	ECall *pCall = NewCall (RFCOMM_DISC, pContext);
	if (! pCall) {
		IFDBG(DebugOut (DEBUG_ERROR, L"[PORTEMU] RFCommDisconnect 0x%08x : Failed to create pending IO. ERROR_NO_SYSTEM_RESOURCES\n", pContext));
		return;
	}

	pCall->pNext = pContext->pCalls;
	pContext->pCalls = pCall;

	if (hConn) {	// Really close it
		IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] RFCommDisconnect 0x%08x : really closing connection to device\n", pContext));
		HANDLE h = pContext->hRFCOMM;
		RFCOMM_Disconnect_In pCallback = pContext->rfcomm_if.rfcomm_Disconnect_In;

		gpPORTEMU->Unlock ();
		__try {
			pCallback (h, pCall, hConn);
		} __except (1) {
			IFDBG(DebugOut (DEBUG_ERROR, L"[PORTEMU] RFCommDisconnect : exception in rfcomm_Disconnect_In\n"));
		}
		gpPORTEMU->Lock ();
	}
}

static void CloseConnection (PORTEMU_CONTEXT *pContext, int iRes, BOOL fSendDisconnect) {
	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] CloseConnection 0x%08x handle 0x%08x device %04x%08x ch 0x%02x send disc %d\n",
					pContext, pContext->hConnection, pContext->b.NAP, pContext->b.SAP, pContext->channel, fSendDisconnect));

	ReinitClosedConnection (pContext);

	RegisterCommEvent (pContext, EV_RLSD, 0);

	PendingIO *pParent = NULL;
	PendingIO *pIO = pContext->pops;

	while (pIO) {
		if ((pIO->op == COMM_READ) || (pIO->op == COMM_WRITE) || (pIO->op == COMM_OPEN) || (pIO->op == COMM_RPN) || (pIO->op == COMM_DISC)) {
			PendingIO *pNext = pIO->pNext;

			pIO->pNext     = NULL;
			pIO->iIoResult = iRes;
			SetEvent (pIO->hEvent);

			if (pParent)
				pParent->pNext = pNext;
			else
				pContext->pops = pNext;

			pIO = pNext;
		} else {
			pParent = pIO;
			pIO = pIO->pNext;
		}
	}

	while (pContext->pbl) {
		if (pContext->pbl->pb->pFree)
			pContext->pbl->pb->pFree (pContext->pbl->pb);
		BD_BUFFER_LIST *pNext = pContext->pbl->pNext;
		delete pContext->pbl;
		pContext->pbl = pNext;
	}

	while (pContext->pCalls) {
		ECall *pNext = pContext->pCalls->pNext;
		delete pContext->pCalls;
		pContext->pCalls = pNext;
	}

	if (fSendDisconnect)
		RFCommDisconnectIn (pContext);
}

static int GetQuota (PORTEMU_CONTEXT *pContext) {
	if (pContext->credit_fc && (pContext->iRecvQuota < (pContext->iMTU * 2 * PORTEMU_CREDITS_LOWEST)))
		return pContext->iMTU * 2 * PORTEMU_CREDITS_LOWEST;

	return pContext->iRecvQuota;
}

static int GiveCredits (PORTEMU_CONTEXT *pContext) {
	if ((! pContext->credit_fc) || (pContext->iGaveCredits > PORTEMU_CREDITS_LOW))
		return 0;

	int nCredits = (GetQuota (pContext) - pContext->iRecvQuotaUsed) / pContext->iMTU - pContext->iGaveCredits;

	return nCredits > 0 ? nCredits : 0;
}

static void SendCredits (PORTEMU_CONTEXT *pContext) {
	SVSUTIL_ASSERT (pContext->credit_fc);

	if (pContext->iGaveCredits > PORTEMU_CREDITS_LOWEST)
		return;

	int nCredits = (GetQuota (pContext) - pContext->iRecvQuotaUsed) / pContext->iMTU - pContext->iGaveCredits;

	if (nCredits <= 0)
		return;

	IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] SendCredits 0x%08x : sending %d credits\n", pContext, nCredits));

	BD_BUFFER *pBuffer = BufferAlloc (pContext->iDeviceHead + pContext->iDeviceTrail);

	if (! pBuffer) {
		IFDBG(DebugOut (DEBUG_ERROR, L"[PORTEMU] SendCredits 0x%08x :  <out of memory>\n", pContext));
		return;
	}

	pBuffer->cStart = pContext->iDeviceHead;
	pBuffer->cEnd   = pBuffer->cSize - pContext->iDeviceTrail;

	SVSUTIL_ASSERT (BufferTotal (pBuffer) == 0);

	pContext->iGaveCredits += nCredits;

	HANDLE h = pContext->hRFCOMM;
	HANDLE hConn = pContext->hConnection;
	RFCOMM_DataDown_In pCallback = pContext->rfcomm_if.rfcomm_DataDown_In;

	gpPORTEMU->Unlock ();
	__try {
		pCallback (h, NULL, hConn, pBuffer, nCredits);
	} __except (1) {
		IFDBG(DebugOut (DEBUG_ERROR, L"[PORTEMU] SendCredits : exception in rfcomm_DataDown_In\n"));
	}

	gpPORTEMU->Lock ();
}

static int SendData (PORTEMU_CONTEXT *pContext) {
	if (pContext->fSending)		// Guard against packets coming out of order
		return ERROR_IO_PENDING;

	for ( ; ; ) {
		IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] SendData to 0x%08x (%04x%08x ch 0x%02)\n",
										pContext, pContext->b.NAP, pContext->b.SAP, pContext->channel));

		// Data queued, but can't send just yet
		if ((pContext->iSendQuotaUsed >= pContext->iSendQuota) || pContext->fc_aggregate ||
			((! pContext->credit_fc) && pContext->fc) || (pContext->credit_fc && (pContext->iHaveCredits <= 0)) ||
			(! pContext->hConnection) || (! pContext->fConnected)) {
			IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] SendData 0x%08x : ERROR_IO_PENDING\n", pContext));
			return ERROR_IO_PENDING;
		}

		//	Compute buffer
		PendingIO *pIO = pContext->pops;
		int	cTotalBytes = 0;
		int cGiveCredits = GiveCredits (pContext);

		while (pIO) {
			if (pIO->op == COMM_WRITE) {
				if ((cTotalBytes + (pIO->cbuffer - pIO->cbuffer_used)) >= (pContext->iMTU - (cGiveCredits != 0))) {
					cTotalBytes = pContext->iMTU - (cGiveCredits != 0);
					break;
				} else
					cTotalBytes += (pIO->cbuffer - pIO->cbuffer_used);
			}
			pIO = pIO->pNext;
		}

		if (cTotalBytes == 0) {
			IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] SendData 0x%08x : ERROR_SUCCESS <nothing left>\n", pContext));
			RegisterCommEvent(pContext, EV_TXEMPTY, 0);
			return ERROR_SUCCESS;
		}

		IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] SendData to 0x%08x : %d bytes, %d credits\n", pContext, cTotalBytes, cGiveCredits));

		BD_BUFFER *pBuffer = BufferAlloc (cTotalBytes + pContext->iDeviceHead + pContext->iDeviceTrail);

		if (! pBuffer) {
			IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] SendData 0x%08x : ERROR_IO_PENDING <out of memory>\n", pContext));
			return ERROR_IO_PENDING;
		}

		ECall *pCall = NewCall (RFCOMM_WRITE, pContext);

		if (! pCall) {
			if (pBuffer->pFree)
				pBuffer->pFree (pBuffer);
			IFDBG(DebugOut (DEBUG_RFCOMM_TRACE, L"[PORTEMU] SendData 0x%08x : ERROR_IO_PENDING <out of memory: call>\n", pContext));
			return ERROR_IO_PENDING;
		}

		pCall->pNext     = pContext->pCalls;
		pContext->pCalls = pCall;
		pCall->cBytes    = cTotalBytes;

		pBuffer->cEnd = pBuffer->cSize - pContext->iDeviceTrail;
		pBuffer->cStart = pContext->iDeviceHead;

		SVSUTIL_ASSERT (BufferTotal (pBuffer) == cTotalBytes);

		// Accumulate buffer
		pIO = pContext->pops;
		PendingIO *pParent = NULL;

		int cHaveBytes = 0;

		while (pIO && (cHaveBytes < cTotalBytes)) {
			if (pIO->op == COMM_WRITE) {
				int cThisChunk = cTotalBytes - cHaveBytes;
				if (cThisChunk > (pIO->cbuffer - pIO->cbuffer_used))
					cThisChunk = pIO->cbuffer - pIO->cbuffer_used;

				SVSUTIL_ASSERT (pIO->dwPerms);
				DWORD dwCurrentPerms = SetProcPermissions (pIO->dwPerms);
				BOOL bkm = SetKMode (TRUE);
				memcpy (pBuffer->pBuffer + pContext->iDeviceHead + cHaveBytes, pIO->buffer + pIO->cbuffer_used, cThisChunk);
				SetKMode (bkm);
				SetProcPermissions (dwCurrentPerms);

				cHaveBytes += cThisChunk;
				pIO->cbuffer_used += cThisChunk;

				if (pIO->cbuffer_used == pIO->cbuffer) {	// Retire the buffer
					PendingIO *pNext = pIO->pNext;
					pIO->iIoResult = ERROR_SUCCESS;
					pIO->pNext = NULL;
					SetEvent (pIO->hEvent);

					if (pParent)
						pParent->pNext = pNext;
					else
						pContext->pops = pNext;

					pIO = pNext;
					continue;
				}

				SVSUTIL_ASSERT (cHaveBytes == cTotalBytes);
				break;
			}
			pParent = pIO;