filter.c

USB filter driver sample

		oldIrql = KeGetCurrentIrql();
		if (oldIrql < DISPATCH_LEVEL)
			{
			ExAcquireFastMutex(&devExt->FMutex);
		//	KeWaitForSingleObject(&devExt->kTimerEvent, Executive, KernelMode, FALSE, &devExt->lnTimer);
			DBGOUT(("Read delay..."));
			}
		else
			{
			DBGOUT(("Irql == DISPATCH_LEVEL"));
			}

		if (wLen + devExt->wReadOffset[bWhich] > MAX_BUFFER)
			{
			bWhich = bWhich ^ 0x01;
			devExt->wReadOffset[bWhich] = 0;
			devExt->bWhichBuffer_read = bWhich;
			}

	//	DBGOUT(("read len : %u(%u)", wLen, bWhich));
		devExt->PkHdr[devExt->nCount].code = wReadFlag;
		devExt->PkHdr[devExt->nCount].length = (WORD)wLen;
		devExt->PkHdr[devExt->nCount].offset = (WORD)devExt->wReadOffset[bWhich];
		devExt->PkHdr[devExt->nCount].bFlag = bWhich;
		GET_TIME(&devExt->PkHdr[devExt->nCount].tv, &devExt->start_time);
		if (devExt->nCount >= MAX_PACKET-1)
			devExt->nCount = 0;
		else
			devExt->nCount++;
		
		RtlCopyMemory(devExt->pReadBuffer[bWhich] + devExt->wReadOffset[bWhich], irp->AssociatedIrp.SystemBuffer, wLen);
		InterlockedExchangeAdd((PLONG)&devExt->wReadOffset[bWhich], wLen);
	//	devExt->wReadOffset[bWhich] += wLen;
		if (oldIrql < DISPATCH_LEVEL)
			ExReleaseFastMutex(&devExt->FMutex);

	//	KeLowerIrql(oldIrql);
		KeReleaseSemaphore(&devExt->pSemaphore, 0, 1, FALSE);
		}

	if (irp->PendingReturned)
		{
		IoMarkIrpPending(irp);
		}

	return irp->IoStatus.Status;
	}

NTSTATUS USB_IoCtrl(struct DEVICE_EXTENSION *devExt, PIRP irp)
	{
	PIO_STACK_LOCATION irpSp = IoGetCurrentIrpStackLocation(irp);
	ULONG dwCode = irpSp->Parameters.DeviceIoControl.IoControlCode;
	struct DEVICE_EXTENSION *filtExt = devExt->filterDevObj->DeviceExtension;
	ULONG dwReadBufferSize = 0, dwTemp = 0;
	EVENT_STRUCT* user_event;
	NTSTATUS status = STATUS_SUCCESS;
//	KIRQL oldIrql;
	UCHAR* pTemp;
	DWORD dwCount;

	switch (dwCode)
		{
		case IOCTL_TEST_BUFFERED:
			RtlCopyMemory(irp->AssociatedIrp.SystemBuffer, &DRIVER_VERSION, sizeof(ULONG));
			irp->IoStatus.Information = sizeof(ULONG);
			break;
			
		case IOCTL_CAPTURE_READ_DATA:	/// obsolete
			break;

		case IOCTL_CAPTURE_WRITE_DATA:	/// obsolete
			break;

		case IOCTL_CAPTURE_DATA:		/// both read and write data
		//	KeRaiseIrql(DISPATCH_LEVEL, &oldIrql);
			ExAcquireFastMutex(&filtExt->FMutex);
			/// copy data
			pTemp = (UCHAR *)MmGetSystemAddressForMdlSafe(irp->MdlAddress, NormalPagePriority);
			dwCount = filtExt->nMergeCount;
			RtlCopyMemory(pTemp, &dwCount, dwUlongSize);
			dwTemp = dwUlongSize;
			RtlCopyMemory(pTemp+dwTemp, filtExt->pCaptureHdr, nPKHDR_SIZE*dwCount);
			dwTemp += nPKHDR_SIZE*dwCount;
			RtlCopyMemory(pTemp+dwTemp, filtExt->pCaptureBuffer, filtExt->dwMergeOffset);
							//filtExt->pCaptureHdr[dwCount-1].offset + filtExt->pCaptureHdr[dwCount-1].length);

			irp->IoStatus.Information = filtExt->dwMergeOffset + dwTemp;
			filtExt->nMergeCount = 0;
			filtExt->dwMergeOffset = 0;
			ExReleaseFastMutex(&filtExt->FMutex);
		//	KeLowerIrql(oldIrql);
			break;

		case IOCTL_CAPTURE_STOP:
			DBGOUT(("Capture stop"));
			if (filtExt->bInit)
				CaptureInit(filtExt, false);

			break;

		case IOCTL_SET_EVENT:
			if (filtExt->bInit)
				CaptureInit(filtExt, false);
			
			user_event = (EVENT_STRUCT *)irp->AssociatedIrp.SystemBuffer;
			status = ObReferenceObjectByHandle(user_event->hPacket, SYNCHRONIZE, NULL, KernelMode, 
												(PVOID *)&filtExt->packet_Event, NULL);
			if (NT_SUCCESS(status))
				{
				status = ObReferenceObjectByHandle(user_event->hRemove, SYNCHRONIZE, NULL, KernelMode,
												(PVOID *)&filtExt->removeEvent_app, NULL);
				if (NT_SUCCESS(status))
					{
					DBGOUT(("Set Event"));
					}
				else
					{
					DBGOUT(("Set Event failed!!!"));
					ObDereferenceObject(filtExt->packet_Event);
					}
				}
			else
				{
				DBGOUT(("Set Event failed!!!"));
				}
			
			irp->IoStatus.Information = 0;
			break;

		case IOCTL_CAPTURE_START:
			status = CaptureInit(filtExt, true);
			break;
			
		default:
			break;
		}

	return status;//STATUS_SUCCESS;
	}

VOID CaptureThread(IN PVOID Context)
	{
	bool bAt = false, bPPP = false, bPacket = false;
	enum AT_RESPONSE eAtRes = STOP_LF;
	int nCurCount = 0, nCapCount = 0;
	bool bWhich = 0, bIndex = 0, bHdr = 0;
	DWORD dwOffset = 0, dwReadSize = 0;
	unsigned char *pTemp, *pReadTemp;
	struct DEVICE_EXTENSION *devExt;
	int i;
	//KIRQL irql;
	pTemp = ExAllocatePool(NonPagedPool, 4096);
	devExt = (struct DEVICE_EXTENSION *)Context;
	KeSetPriorityThread(KeGetCurrentThread(), LOW_REALTIME_PRIORITY);

	DBGOUT(("CreateThread..."));
	while (1)
		{
		KeWaitForSingleObject(&devExt->pSemaphore, Executive, KernelMode, FALSE, NULL);
		if (devExt->bThreadStop)
			{
			ExFreePool(pTemp);
			PsTerminateSystemThread(STATUS_SUCCESS);
			}

		/// packet 贸府...
		bPacket = false;
			
		bWhich = (bool)devExt->PkHdr[nCapCount].bFlag;
		if (devExt->PkHdr[nCapCount].code == wWriteFlag)
			{
			ExAcquireFastMutex(&devExt->FMutex);
			devExt->pCaptureHdr[devExt->nMergeCount].code = devExt->PkHdr[nCapCount].code;
			devExt->pCaptureHdr[devExt->nMergeCount].length = devExt->PkHdr[nCapCount].length;
			devExt->pCaptureHdr[devExt->nMergeCount].offset = devExt->dwTotalOffset;
			devExt->pCaptureHdr[devExt->nMergeCount].tv = devExt->PkHdr[nCapCount].tv;
			RtlCopyMemory(devExt->pCaptureBuffer + devExt->dwMergeOffset, 
						  devExt->pWriteBuffer[bWhich] + devExt->PkHdr[nCapCount].offset, 
						  devExt->PkHdr[nCapCount].length);

		//	InterlockedExchangeAdd(&devExt->dwTotalOffset, devExt->PkHdr[nCapCount].length);
		//	InterlockedExchangeAdd(&devExt->dwMergeOffset, devExt->PkHdr[nCapCount].length);
			devExt->dwTotalOffset += devExt->PkHdr[nCapCount].length;
			devExt->dwMergeOffset += devExt->PkHdr[nCapCount].length;
			if (devExt->nMergeCount >= MAX_PACKET-1)
				{
				devExt->nMergeCount = 0;
				devExt->dwMergeOffset = 0;
				}
			else
				devExt->nMergeCount++;

			ExReleaseFastMutex(&devExt->FMutex);
			KeSetEvent(devExt->packet_Event, 0, FALSE);
		//	DBGOUT(("write packet merge..."));
			}
		else
			{
			pReadTemp = devExt->pReadBuffer[bWhich] + devExt->PkHdr[nCapCount].offset;
			for (i = 0; i < devExt->PkHdr[nCapCount].length; i++)
				{
				if (bPPP)
					{
					if (pReadTemp[i] == 0x7E && eAtRes == STOP_LF)
						{
						bPPP = false;
						bPacket = true;
						}
					}
				else
					{
					switch (pReadTemp[i])
						{
						case 0x7E:
							if (eAtRes == STOP_LF)
								{
								bPPP = true;
							//	dwReadSize = 0;
								}
							break;

						case 'A':
							if (bAt == false && eAtRes != START_LF)
								{
								bAt = true;
							//	dwReadSize = 0;
								}
							break;

						case 0x0D:
							if (bAt == true)
								{
								bAt = false;
								eAtRes = START_CR;
								bPacket = true;
								}
							else
								{
								switch (eAtRes)
									{
									case START_LF:
										eAtRes = STOP_CR;
										break;

									case STOP_LF:
										eAtRes = START_CR;
									//	dwReadSize = 0;
										break;
										
									case SEPARATOR:
										eAtRes = START_LF;
										break;
									}
								}
							break;

						case 0x0A:
							switch (eAtRes)
								{
								case STOP_CR:
									eAtRes = STOP_LF;
									bPacket = true;
									break;

								case START_CR:
									eAtRes = START_LF;
									break;

								default:
									eAtRes = SEPARATOR;
									break;
								}
							break;

						default:
							if (bAt == false && eAtRes != START_LF)
								{
							//	eAtRes = START_LF;
							//	dwReadSize = 0;
								DBGOUT(("ppp packet loss!!!"));
								pTemp[dwReadSize] = 0x7E;	/// PPP loss...
								dwReadSize++;
								bPPP = true;
								}
							break;
						}
					}

				if (dwReadSize > 4094)
					{
					DBGOUT(("dwReadSize : %u", dwReadSize));
					bPacket = true;
					bPPP = true;
					}

				pTemp[dwReadSize] = pReadTemp[i];
				dwReadSize++;
				if (bPacket)
					{
				//	KeRaiseIrql(DISPATCH_LEVEL, &irql);
					ExAcquireFastMutex(&devExt->FMutex);
					devExt->pCaptureHdr[devExt->nMergeCount].code = devExt->PkHdr[nCapCount].code;
					devExt->pCaptureHdr[devExt->nMergeCount].offset = devExt->dwTotalOffset;
					devExt->pCaptureHdr[devExt->nMergeCount].length = (WORD)dwReadSize;
					devExt->pCaptureHdr[devExt->nMergeCount].tv = devExt->PkHdr[nCapCount].tv;
					RtlCopyMemory(devExt->pCaptureBuffer + devExt->dwMergeOffset, pTemp, dwReadSize);
					InterlockedExchangeAdd(&devExt->dwTotalOffset, dwReadSize);
					InterlockedExchangeAdd(&devExt->dwMergeOffset, dwReadSize);
					if (devExt->nMergeCount >= MAX_PACKET-1)
						{
						devExt->nMergeCount = 0;
						devExt->dwMergeOffset = 0;
						}
					else
						devExt->nMergeCount++;

					ExReleaseFastMutex(&devExt->FMutex);
				//	KeLowerIrql(irql);
					KeSetEvent(devExt->packet_Event, 0, FALSE);
					bPacket = false;
					dwReadSize = 0;
				//	DBGOUT(("read packet merge..."));
					}
				}
			}

		if (nCapCount >= MAX_PACKET-1)
			nCapCount = 0;
		else
			nCapCount++;

	//	irql = KeGetCurrentIrql();
	//	DBGOUT(("Capture thread len : %u", irql));
		}
	}

void KillThread(IN struct DEVICE_EXTENSION *devExt)
	{
	DBGOUT(("KillThread..."));
	devExt->bThreadStop = true;
	KeReleaseSemaphore(&devExt->pSemaphore, 0, 1, TRUE);
	KeWaitForSingleObject(&devExt->pThread, Executive, KernelMode, FALSE, NULL);
	ObDereferenceObject(&devExt->pThread);
	devExt->pThread = NULL;
	}

NTSTATUS CaptureInit(struct DEVICE_EXTENSION *devExt, bool bInit)
	{
	NTSTATUS status = STATUS_SUCCESS;
	if (bInit)
		{
		HANDLE hThread;
		TIME_DESYNCHRONIZE(&devExt->start_time);
		TIME_SYNCHRONIZE(&devExt->start_time);
		devExt->bThreadStop = false;
		status = PsCreateSystemThread(&hThread, 0, NULL, NULL, NULL, (PKSTART_ROUTINE)CaptureThread, devExt);
		if (NT_SUCCESS(status))
			{
			ObReferenceObjectByHandle(hThread, THREAD_ALL_ACCESS, NULL, KernelMode, (PVOID *)&devExt->pThread, NULL);
			ZwClose(hThread);
		
			devExt->pWriteBuffer[0] = ExAllocatePool(NonPagedPool, MAX_BUFFER);
			devExt->pWriteBuffer[1] = ExAllocatePool(NonPagedPool, MAX_BUFFER);
			devExt->pReadBuffer[0] = ExAllocatePool(NonPagedPool, MAX_BUFFER);
			devExt->pReadBuffer[1] = ExAllocatePool(NonPagedPool, MAX_BUFFER);
			devExt->pCaptureBuffer = ExAllocatePool(NonPagedPool, MAX_CAPTURE_BUFFER);
			devExt->pCaptureHdr = ExAllocatePool(NonPagedPool, nPKHDR_SIZE*MAX_PACKET);

			devExt->wWriteOffset[0] = 0;
			devExt->wWriteOffset[1] = 0;
			devExt->wReadOffset[0] = 0;
			devExt->wReadOffset[1] = 0;

			devExt->bWhichBuffer_read = 0;
			devExt->bWhichBuffer_write = 0;

			devExt->nCount = devExt->nMergeCount = 0;
			devExt->dwTotalOffset = devExt->dwMergeOffset = 0;
			devExt->readflag = CAPTURE_START;
			devExt->writeflag = CAPTURE_START;
			DBGOUT(("CaptureInit..."));
			devExt->bInit = true;
			}
		}
	else
		{
		DBGOUT(("Capture Release..."));
		devExt->readflag = CAPTURE_STOP;
		devExt->writeflag = CAPTURE_STOP;
		if (devExt->bThreadStop == false)
			KillThread(devExt);

		if (devExt->packet_Event)
			{
			ObDereferenceObject(devExt->packet_Event);
			devExt->packet_Event = NULL;
			}

		if (devExt->removeEvent_app)
			{
			ObDereferenceObject(devExt->removeEvent_app);
			devExt->removeEvent_app = NULL;
			}

		if (devExt->pWriteBuffer[0])
			ExFreePool(devExt->pWriteBuffer[0]);

		if (devExt->pWriteBuffer[1])
			ExFreePool(devExt->pWriteBuffer[1]);

		if (devExt->pReadBuffer[0])
			ExFreePool(devExt->pReadBuffer[0]);

		if (devExt->pReadBuffer[1])
			ExFreePool(devExt->pReadBuffer[1]);

		if (devExt->pCaptureBuffer)
			ExFreePool(devExt->pCaptureBuffer);

		if (devExt->pCaptureHdr)
			ExFreePool(devExt->pCaptureHdr);
		
		devExt->pWriteBuffer[0] = devExt->pWriteBuffer[1] = devExt->pReadBuffer[0] = devExt->pReadBuffer[1] = NULL;
		devExt->pCaptureBuffer = NULL;
		devExt->pCaptureHdr = NULL;

		devExt->wWriteOffset[0] = 0;
		devExt->wWriteOffset[1] = 0;
		devExt->wReadOffset[0] = 0;
		devExt->wReadOffset[1] = 0;
		devExt->bInit = false;
		TIME_DESYNCHRONIZE(&devExt->start_time);
		}

	return status;
	}

VOID TimerDpc(IN PKDPC pDpc, IN PVOID pContext, IN PVOID SysArg1, IN PVOID SysArg2)
	{
	struct DEVICE_EXTENSION* pDevExt = (struct DEVICE_EXTENSION*)pContext;
//	if (pDevExt)
	KeSetEvent(&pDevExt->kTimerEvent, 0, FALSE);
	}