NTSTATUS WinIoDispatch(IN PDEVICE_OBJECT DeviceObject, IN PIRP Irp) { PIO_STACK_LOCATION IrpStack; ULONG dwInputBufferLength; ULONG dwOutputBufferLength; ULONG dwIoControlCode; PVOID pvIOBuffer; NTSTATUS ntStatus; struct tagPhys32Struct Phys32Struct; OutputDebugString ("Entering WinIoDispatch"); // Init to default settings Irp->IoStatus.Status = STATUS_SUCCESS; Irp->IoStatus.Information = 0; IrpStack = IoGetCurrentIrpStackLocation(Irp); // Get the pointer to the input/output buffer and it's length pvIOBuffer = Irp->AssociatedIrp.SystemBuffer; dwInputBufferLength = IrpStack->Parameters.DeviceIoControl.InputBufferLength; dwOutputBufferLength = IrpStack->Parameters.DeviceIoControl.OutputBufferLength; switch (IrpStack->MajorFunction) { case IRP_MJ_CREATE: OutputDebugString("IRP_MJ_CREATE"); break; case IRP_MJ_CLOSE: OutputDebugString("IRP_MJ_CLOSE"); break; case IRP_MJ_DEVICE_CONTROL: OutputDebugString("IRP_MJ_DEVICE_CONTROL"); dwIoControlCode = IrpStack->Parameters.DeviceIoControl.IoControlCode; switch (dwIoControlCode) { case IOCTL_WINIO_ENABLEDIRECTIO: OutputDebugString("IOCTL_WINIO_ENABLEDIRECTIO"); pIOPM = MmAllocateNonCachedMemory(sizeof(IOPM)); if (pIOPM) { RtlZeroMemory(pIOPM, sizeof(IOPM)); Ke386IoSetAccessProcess(PsGetCurrentProcess(), 1); Ke386SetIoAccessMap(1, pIOPM); } else Irp->IoStatus.Status = STATUS_INSUFFICIENT_RESOURCES; break; case IOCTL_WINIO_DISABLEDIRECTIO: OutputDebugString("IOCTL_WINIO_DISABLEDIRECTIO"); if (pIOPM) { Ke386IoSetAccessProcess(PsGetCurrentProcess(), 0); Ke386SetIoAccessMap(1, pIOPM); MmFreeNonCachedMemory(pIOPM, sizeof(IOPM)); pIOPM = NULL; } break; case IOCTL_WINIO_MAPPHYSTOLIN: OutputDebugString("IOCTL_WINIO_MAPPHYSTOLIN"); if (dwInputBufferLength) { memcpy (&Phys32Struct, pvIOBuffer, dwInputBufferLength); ntStatus = MapPhysicalMemoryToLinearSpace(Phys32Struct.pvPhysAddress, Phys32Struct.dwPhysMemSizeInBytes, &Phys32Struct.pvPhysMemLin, &Phys32Struct.PhysicalMemoryHandle); if (NT_SUCCESS(ntStatus)) { memcpy (pvIOBuffer, &Phys32Struct, dwInputBufferLength); Irp->IoStatus.Information = dwInputBufferLength; } Irp->IoStatus.Status = ntStatus; } else Irp->IoStatus.Status = STATUS_INVALID_PARAMETER; break; case IOCTL_WINIO_UNMAPPHYSADDR: OutputDebugString("IOCTL_WINIO_UNMAPPHYSADDR"); if (dwInputBufferLength) { memcpy (&Phys32Struct, pvIOBuffer, dwInputBufferLength); ntStatus = UnmapPhysicalMemory(Phys32Struct.PhysicalMemoryHandle, Phys32Struct.pvPhysMemLin); Irp->IoStatus.Status = ntStatus; } else Irp->IoStatus.Status = STATUS_INVALID_PARAMETER; break; default: OutputDebugString("ERROR: Unknown IRP_MJ_DEVICE_CONTROL"); Irp->IoStatus.Status = STATUS_INVALID_PARAMETER; break; } break; } // DON'T get cute and try to use the status field of the irp in the // return status. That IRP IS GONE as soon as you call IoCompleteRequest. ntStatus = Irp->IoStatus.Status; IoCompleteRequest (Irp, IO_NO_INCREMENT); // We never have pending operation so always return the status code. OutputDebugString("Leaving WinIoDispatch"); return ntStatus; }
NTSTATUS hwinterfaceDeviceControl(IN PDEVICE_OBJECT DeviceObject, IN PIRP pIrp) { PIO_STACK_LOCATION stkloc; NTSTATUS ntStatus = STATUS_SUCCESS; struct tagPhys32Struct Phys32Struct; PUCHAR cData; PUSHORT sData; PULONG lData; PUSHORT address; ULONG inBuffersize; ULONG outBuffersize; ULONG inBuf; PVOID CtrlBuff; stkloc = IoGetCurrentIrpStackLocation( pIrp ); inBuffersize = stkloc->Parameters.DeviceIoControl.InputBufferLength; outBuffersize = stkloc->Parameters.DeviceIoControl.OutputBufferLength; CtrlBuff = pIrp->AssociatedIrp.SystemBuffer; cData = (PUCHAR) CtrlBuff; sData = (PUSHORT) CtrlBuff; lData = (PULONG) CtrlBuff; address = (PUSHORT) CtrlBuff; switch ( stkloc->Parameters.DeviceIoControl.IoControlCode ) { case IOCTL_READ_PORT_UCHAR: if ((inBuffersize >= 2) && (outBuffersize >= 1)) { UCHAR value; value = READ_PORT_UCHAR((PUCHAR)address[0]); cData[0] = value; } else { ntStatus = STATUS_BUFFER_TOO_SMALL; } pIrp->IoStatus.Information = sizeof(UCHAR); ntStatus = STATUS_SUCCESS; break; case IOCTL_WRITE_PORT_UCHAR: if (inBuffersize >= 3) { WRITE_PORT_UCHAR((PUCHAR)address[0], cData[2]); //Byte 0,1=Address Byte 2=Value pIrp->IoStatus.Information = 10; } else { ntStatus = STATUS_BUFFER_TOO_SMALL; pIrp->IoStatus.Information = 0; ntStatus = STATUS_SUCCESS; } break; case IOCTL_READ_PORT_USHORT: if ((inBuffersize >= 2) && (outBuffersize >= 2)) { USHORT value; value = READ_PORT_USHORT((PUSHORT)address[0]); sData[0] = value; } else { ntStatus = STATUS_BUFFER_TOO_SMALL; } pIrp->IoStatus.Information = sizeof(USHORT); ntStatus = STATUS_SUCCESS; break; case IOCTL_WRITE_PORT_USHORT: if (inBuffersize >= 4) { WRITE_PORT_USHORT((PUSHORT)address[0], sData[1]); //Short 0=Address Short 1=Value pIrp->IoStatus.Information = 10; } else { ntStatus = STATUS_BUFFER_TOO_SMALL; pIrp->IoStatus.Information = 0; ntStatus = STATUS_SUCCESS; } break; case IOCTL_READ_PORT_ULONG: if ((inBuffersize >= 4) && (outBuffersize >= 4)) { ULONG value; value = READ_PORT_ULONG((PULONG)address[0]); lData[0] = value; } else { ntStatus = STATUS_BUFFER_TOO_SMALL; } pIrp->IoStatus.Information = sizeof(ULONG); ntStatus = STATUS_SUCCESS; break; case IOCTL_WRITE_PORT_ULONG: if (inBuffersize >= 8) { WRITE_PORT_ULONG(&(lData[0]), lData[1]); //Short 0=Address long 1=Value pIrp->IoStatus.Information = 10; } else { ntStatus = STATUS_BUFFER_TOO_SMALL; pIrp->IoStatus.Information = 0; ntStatus = STATUS_SUCCESS; } break; case IOCTL_WINIO_MAPPHYSTOLIN: if (inBuffersize) { memcpy (&Phys32Struct, CtrlBuff, inBuffersize); ntStatus = MapPhysicalMemoryToLinearSpace(Phys32Struct.pvPhysAddress, Phys32Struct.dwPhysMemSizeInBytes, &Phys32Struct.pvPhysMemLin, &Phys32Struct.PhysicalMemoryHandle); if (NT_SUCCESS(ntStatus)) { memcpy (CtrlBuff, &Phys32Struct, inBuffersize); pIrp->IoStatus.Information = inBuffersize; } pIrp->IoStatus.Status = ntStatus; } else pIrp->IoStatus.Status = STATUS_INVALID_PARAMETER; break; case IOCTL_WINIO_UNMAPPHYSADDR: if (inBuffersize) { memcpy (&Phys32Struct, CtrlBuff, inBuffersize); ntStatus = UnmapPhysicalMemory(Phys32Struct.PhysicalMemoryHandle, Phys32Struct.pvPhysMemLin); pIrp->IoStatus.Status = ntStatus; } else pIrp->IoStatus.Status = STATUS_INVALID_PARAMETER; break; default: ntStatus = STATUS_UNSUCCESSFUL; pIrp->IoStatus.Information = 0; break; } pIrp->IoStatus.Status = ntStatus; IoCompleteRequest( pIrp, IO_NO_INCREMENT ); return ntStatus; }