From: Rolf K. <lab...@us...> - 2008-11-15 21:45:32
|
Update of /cvsroot/opengtoolkit/portIO/c_source In directory 23jxhf1.ch3.sourceforge.com:/tmp/cvs-serv525 Modified Files: ogportio.c Log Message: Bugfix in direct memory and port access Index: ogportio.c =================================================================== RCS file: /cvsroot/opengtoolkit/portIO/c_source/ogportio.c,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** ogportio.c 15 Nov 2007 09:12:57 -0000 1.3 --- ogportio.c 15 Nov 2008 21:45:26 -0000 1.4 *************** *** 42,53 **** VOID OGPortIOUnload(IN PDRIVER_OBJECT driverObject); - #ifdef MAP_ADDRESS - #define BusAddress(busType, bus, physAddr, memType, mapAddr) \ - HalTranslateBusAddress(Isa, 0, physAddr, &memType, &mapAddr) - #else - #define BusAddress(busType, bus, physAddr, memType, mapAddr) \ - (mapAddr.LowPart = physAddr.LowPart, mapAddr.HighPart = physAddr.HighPart, TRUE) - #endif - #ifdef ALLOC_PRAGMA #pragma alloc_text (INIT, DriverEntry) --- 42,45 ---- *************** *** 137,478 **** } ! NTSTATUS OGPortIODispatch(IN PDEVICE_OBJECT pDevObj, IN PIRP pIrp) { ! NTSTATUS ntStatus = STATUS_SUCCESS; ! ! PUCHAR pIOBuffer = (PUCHAR)pIrp->AssociatedIrp.SystemBuffer; ! PPORTMAP portmap = pIrp->AssociatedIrp.SystemBuffer; ! PPORTREG portreg = pIrp->AssociatedIrp.SystemBuffer; ! PMEMMAP memmap = pIrp->AssociatedIrp.SystemBuffer; ! ! PIO_STACK_LOCATION irpSp = IoGetCurrentIrpStackLocation(pIrp); ! ULONG inBufLength = irpSp->Parameters.DeviceIoControl.InputBufferLength; ! ULONG outBufLength = irpSp->Parameters.DeviceIoControl.OutputBufferLength; ! ! switch (irpSp->Parameters.DeviceIoControl.IoControlCode) { ! case IOCTL_GET_VERSION: ! KdPrint(("OpenG PORTIO: IOCTL_GET_VERSION\n")); ! if (outBufLength >= sizeof(ULONG)) { ! *(PULONG)pIOBuffer = OGPORTIO_VERSION << 16; ! pIrp->IoStatus.Information = sizeof(ULONG); /* output buffer size */ } else { ! ntStatus = STATUS_BUFFER_TOO_SMALL; ! pIrp->IoStatus.Information = 0; /* output buffer size */ } ! break; ! case IOCTL_GET_IOPM: ! KdPrint(("OpenG PORTIO: IOCTL_GET_IOPM\n")); ! ntStatus = OGPortIOProcess(inBufLength >= 4 ? portmap->processID : 0, TRUE); ! if (ntStatus == STATUS_SUCCESS) ! { ! Ke386QueryIoAccessMap(1, pDevObj->DeviceExtension); ! if (outBufLength > 0) ! { ! PUCHAR buf = (PUCHAR)(pDevObj->DeviceExtension); ! if (inBufLength >= 8) ! { ! if (outBufLength > (portmap->offset + IOPM_SIZE)) ! outBufLength = IOPM_SIZE - portmap->offset; ! buf += portmap->offset; ! } ! else ! { ! if (outBufLength > IOPM_SIZE) ! outBufLength = IOPM_SIZE; ! } ! RtlMoveMemory(pIOBuffer, buf, outBufLength); ! } ! } ! pIrp->IoStatus.Information = outBufLength; /* output buffer size */ ! break; ! case IOCTL_SET_IOPM: ! KdPrint(("OpenG PORTIO: IOCTL_SET_IOPM\n")); ! ntStatus = OGPortIOProcess(inBufLength >= 4 ? portmap->processID : 0, TRUE); ! if (ntStatus == STATUS_SUCCESS) ! { ! if (inBufLength > 8 && portmap->offset < IOPM_SIZE) ! { ! PUCHAR buf = (PUCHAR)(pDevObj->DeviceExtension) + portmap->offset; ! inBufLength -= 8; ! if (portmap->offset + inBufLength > IOPM_SIZE) ! inBufLength = IOPM_SIZE - portmap->offset; ! RtlMoveMemory(buf, portmap->iopm, inBufLength); ! } ! Ke386SetIoAccessMap(1, pDevObj->DeviceExtension); } ! pIrp->IoStatus.Information = 0; /* output buffer size */ ! break; ! case IOCTL_RESET_IOPM: ! KdPrint(("OpenG PORTIO: IOCTL_RESET_IOPM\n")); ! ntStatus = OGPortIOProcess(inBufLength >= 4 ? portmap->processID : 0, FALSE); ! pIrp->IoStatus.Information = 0; /* output buffer size */ ! break; ! case IOCTL_READ_PORT: ! if ((inBufLength >= 8) && (outBufLength >= portreg->size)) { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! PVOID logicAddr; ! ULONG memType = 1; ! KdPrint(("OpenG PORTIO: IOCTL_READ_PORT 0x%X, size %d\n", portreg->address, portreg->size)); ! physAddr.LowPart = portreg->address; ! physAddr.HighPart = 0; ! if (HalTranslateBusAddress(Isa, 0, physAddr, memType, mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_READ_PORT\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else ! { ! logicAddr = (PVOID)mappedAddr.LowPart; ! } ! if (ntStatus == STATUS_SUCCESS) ! { ! switch (portreg->size) ! { ! case 1: ! ((PUCHAR)pIOBuffer)[0] = READ_PORT_UCHAR(logicAddr); ! KdPrint(("OpenG PORTIO: Value read %X\n", ((PUCHAR)pIOBuffer)[0])); ! break; ! case 2: ! ((PUSHORT)pIOBuffer)[0] = READ_PORT_USHORT(logicAddr); ! KdPrint(("OpenG PORTIO: Value read %X\n", ((PUSHORT)pIOBuffer)[0])); ! break; ! case 4: ! ((PULONG)pIOBuffer)[0] = READ_PORT_ULONG(logicAddr); ! KdPrint(("OpenG PORTIO: Value read %X\n", ((PULONG)pIOBuffer)[0])); ! break; ! default: ! KdPrint(("OpenG PORTIO: Invalid port read size\n")); ! ntStatus = STATUS_INVALID_PARAMETER; ! break; ! } ! } ! } ! else { ! ntStatus = STATUS_BUFFER_TOO_SMALL; } if (ntStatus == STATUS_SUCCESS) ! pIrp->IoStatus.Information = portreg->size; /* output buffer size */ ! else ! pIrp->IoStatus.Information = 0; ! break; ! case IOCTL_WRITE_PORT: ! if (inBufLength >= portreg->size + 8) { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! PVOID logicAddr; ! ULONG memType = 1; ! KdPrint(("OpenG PORTIO: IOCTL_WRITE_PORT 0x%X, size %d\n", portreg->address, portreg->size)); ! physAddr.LowPart = portreg->address; ! physAddr.HighPart = 0; ! if (HalTranslateBusAddress(Isa, 0, physAddr, memType, mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_WRITE_PORT\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else ! { ! logicAddr = (PVOID)mappedAddr.LowPart; ! } ! if (ntStatus == STATUS_SUCCESS) ! { ! switch (portreg->size) ! { ! case 1: ! KdPrint(("OpenG PORTIO: Value write 0x%X\n", portreg->u.byte)); ! WRITE_PORT_UCHAR(logicAddr, portreg->u.byte); ! break; ! case 2: ! KdPrint(("OpenG PORTIO: Value write 0x%X\n", portreg->u.word)); ! WRITE_PORT_USHORT(logicAddr, portreg->u.word); ! break; ! case 4: ! KdPrint(("OpenG PORTIO: Value write 0x%X\n", portreg->u.dword)); ! WRITE_PORT_ULONG(logicAddr, portreg->u.dword); ! break; ! default: ! KdPrint(("OpenG PORTIO: Invalid port write size\n")); ! ntStatus = STATUS_INVALID_PARAMETER; ! break; ! } ! } ! } ! else { ! ntStatus = STATUS_BUFFER_TOO_SMALL; } ! pIrp->IoStatus.Information = 0; /* output buffer size */ ! break; ! ! case IOCTL_READ_PHYSMEM: ! if ((inBufLength >= sizeof(MEMMAP)) && (outBufLength >= memmap->size)) { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! PVOID logicAddr; ! ULONG memType = 0; ! KdPrint(("OpenG PORTIO: IOCTL_READ_PHYSMEM 0x%X, size %d\n", memmap->address, memmap->size)); ! physAddr.LowPart = memmap->address; ! physAddr.HighPart = 0; ! if (HalTranslateBusAddress(Isa, 0, physAddr, memType, mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_READ_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else if (memType == 0) ! { ! logicAddr = MmMapIoSpace(mappedAddr, memmap->size, FALSE); ! if (logicAddr == 0) ! { ! KdPrint(("MmMapIoSpace failed in IOCTL_READ_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! } ! else ! { ! logicAddr = (PVOID)mappedAddr.LowPart; ! } ! if (ntStatus == STATUS_SUCCESS) ! { ! switch (memmap->unit) ! { ! case 1: ! KdPrint(("OpenG PORTIO: Read memory, unit 1, size %d\n", memmap->size)); ! READ_REGISTER_BUFFER_UCHAR(logicAddr, pIOBuffer, memmap->size); ! break; ! case 2: ! KdPrint(("OpenG PORTIO: Read memory, unit 2, size %d\n", memmap->size/2)); ! READ_REGISTER_BUFFER_USHORT(logicAddr, (PUSHORT)pIOBuffer, memmap->size/2); ! break; ! case 4: ! KdPrint(("OpenG PORTIO: Read memory, unit 4, size %d\n", memmap->size/4)); ! READ_REGISTER_BUFFER_ULONG(logicAddr, (PULONG)pIOBuffer, memmap->size/4); ! break; ! default: ! KdPrint(("OpenG PORTIO: Invalid memory read unit %d\n", memmap->unit)); ! ntStatus = STATUS_INVALID_PARAMETER; ! break; ! } ! if (memType == 0) ! { ! MmUnmapIoSpace(logicAddr, memmap->size); ! } ! } ! } ! else ! { ! ntStatus = STATUS_BUFFER_TOO_SMALL; ! } ! pIrp->IoStatus.Information = 0; /* output buffer size */ break; ! case IOCTL_WRITE_PHYSMEM: ! if (inBufLength >= sizeof(MEMMAP) + memmap->size) ! { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! PVOID logicAddr; ! ULONG memType = 0; ! KdPrint(("OpenG PORTIO: IOCTL_WRITE_PHYSMEM 0x%X, size %d\n", memmap->address, memmap->size)); ! physAddr.LowPart = memmap->address; ! physAddr.HighPart = 0; ! if (HalTranslateBusAddress(Isa, 0, physAddr, memType, mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_WRITE_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else if (memType == 0) ! { ! logicAddr = MmMapIoSpace(mappedAddr, memmap->size, FALSE); ! if (logicAddr == 0) ! { ! KdPrint(("MmMapIoSpace failed in IOCTL_WRITE_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! } ! else ! { ! logicAddr = (PVOID)mappedAddr.LowPart; ! } ! if (ntStatus == STATUS_SUCCESS) ! { ! switch (memmap->unit) ! { ! case 1: ! KdPrint(("OpenG PORTIO: Write memory, unit 1, size %d\n", memmap->size)); ! WRITE_REGISTER_BUFFER_UCHAR(logicAddr, memmap->buf, memmap->size); ! break; ! case 2: ! KdPrint(("OpenG PORTIO: Write memory, unit 2, size %d\n", memmap->size/2)); ! WRITE_REGISTER_BUFFER_USHORT(logicAddr, (PUSHORT)memmap->buf, memmap->size/2); ! break; ! case 4: ! KdPrint(("OpenG PORTIO: Write memory, unit 4, size %d\n", memmap->size/4)); ! WRITE_REGISTER_BUFFER_ULONG(logicAddr, (PULONG)memmap->buf, memmap->size/4); ! break; ! default: ! KdPrint(("OpenG PORTIO: Invalid memory write unit %d\n", memmap->unit)); ! ntStatus = STATUS_INVALID_PARAMETER; ! break; ! } ! if (memType == 0) ! { ! MmUnmapIoSpace(logicAddr, memmap->size); ! } ! } ! } ! else ! { ! ntStatus = STATUS_BUFFER_TOO_SMALL; ! } ! pIrp->IoStatus.Information = 0; /* output buffer size */ break; ! default: ! KdPrint(("OpenG PORTIO: Unsupported IOCTL Call\n")); ntStatus = STATUS_UNSUCCESSFUL; pIrp->IoStatus.Information = 0; --- 129,534 ---- } ! NTSTATUS OGPortIOVersion(PULONG version, ULONG inBufLength, PULONG pOutBufLength) { ! if (inBufLength >= sizeof(ULONG)) ! { ! *version = OGPORTIO_VERSION << 16; ! *pOutBufLength = sizeof(ULONG); /* output buffer size */ ! } ! else ! { ! *pOutBufLength = 0; /* output buffer size */ ! return STATUS_BUFFER_TOO_SMALL; ! } ! return STATUS_SUCCESS; ! } ! NTSTATUS OGPortIOGetPermMap(PDEVICE_OBJECT pDevObj, PPORTMAP portmap, ULONG inBufLength, ! PUCHAR pBuffer, ULONG bufLength, PULONG pOutBufLength) ! { ! NTSTATUS ntStatus = OGPortIOProcess(bufLength >= 4 ? portmap->processID : 0, TRUE); ! *pOutBufLength = 0; ! if (ntStatus == STATUS_SUCCESS) ! { ! Ke386QueryIoAccessMap(1, pDevObj->DeviceExtension); ! if (bufLength > 0) ! { ! PUCHAR buf = (PUCHAR)(pDevObj->DeviceExtension); ! if (inBufLength >= 8) { ! if (bufLength > (portmap->offset + IOPM_SIZE)) ! bufLength = IOPM_SIZE - portmap->offset; ! buf += portmap->offset; } else { ! if (bufLength > IOPM_SIZE) ! bufLength = IOPM_SIZE; } ! RtlMoveMemory(pBuffer, buf, bufLength); ! } ! *pOutBufLength = bufLength; /* output buffer size */ ! } ! return ntStatus; ! } ! NTSTATUS OGPortIOSetPermMap(PDEVICE_OBJECT pDevObj, PPORTMAP portmap, ULONG inBufLength, PULONG pOutBufLength) ! { ! NTSTATUS ntStatus = OGPortIOProcess(inBufLength >= 4 ? portmap->processID : 0, TRUE); ! *pOutBufLength = 0; ! if (ntStatus == STATUS_SUCCESS) ! { ! if (inBufLength > 8 && portmap->offset < IOPM_SIZE) ! { ! PUCHAR buf = (PUCHAR)(pDevObj->DeviceExtension) + portmap->offset; ! inBufLength -= 8; ! if (portmap->offset + inBufLength > IOPM_SIZE) ! inBufLength = IOPM_SIZE - portmap->offset; ! RtlMoveMemory(buf, portmap->iopm, inBufLength); ! } ! Ke386SetIoAccessMap(1, pDevObj->DeviceExtension); ! } ! return ntStatus; ! } ! NTSTATUS OGPortIOReadPort(PPORTREG portreg, ULONG inBufLength, ! PUCHAR pBuffer, ULONG outBufLength, PULONG pOutBufLength) ! { ! NTSTATUS ntStatus = STATUS_BUFFER_TOO_SMALL; ! *pOutBufLength = 0; ! if ((inBufLength >= 8) && (outBufLength >= portreg->size)) ! { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! ULONG memType = 1, size = portreg->size; ! KdPrint(("OpenG PORTIO: IOCTL_READ_PORT 0x%X, size %d\n", portreg->address, portreg->size)); ! physAddr.LowPart = portreg->address; ! physAddr.HighPart = 0; ! if (HalTranslateBusAddress(Isa, 0, physAddr, &memType, &mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_READ_PORT\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else ! { ! PVOID logicAddr = (PVOID)mappedAddr.LowPart; ! ntStatus = STATUS_SUCCESS; ! switch (size) ! { ! case 1: ! ((PUCHAR)pBuffer)[0] = READ_PORT_UCHAR(logicAddr); ! KdPrint(("OpenG PORTIO: Value read %X\n", ((PUCHAR)pBuffer)[0])); ! break; ! case 2: ! ((PUSHORT)pBuffer)[0] = READ_PORT_USHORT(logicAddr); ! KdPrint(("OpenG PORTIO: Value read %X\n", ((PUSHORT)pBuffer)[0])); ! break; ! case 4: ! ((PULONG)pBuffer)[0] = READ_PORT_ULONG(logicAddr); ! KdPrint(("OpenG PORTIO: Value read %X\n", ((PULONG)pBuffer)[0])); ! break; ! default: ! KdPrint(("OpenG PORTIO: Invalid port read size\n")); ! ntStatus = STATUS_INVALID_PARAMETER; ! break; } + if (ntStatus == STATUS_SUCCESS) + *pOutBufLength = size; /* output buffer size */ + } + } + return ntStatus; + } ! NTSTATUS OGPortIOWritePort(PPORTREG portreg, ULONG inBufLength, ULONG *pOutBufLength) ! { ! NTSTATUS ntStatus = STATUS_BUFFER_TOO_SMALL; ! *pOutBufLength = 0; /* output buffer size */ ! if (inBufLength >= portreg->size + 8) ! { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! ULONG memType = 1, size = portreg->size; ! KdPrint(("OpenG PORTIO: IOCTL_WRITE_PORT 0x%X, size %d\n", portreg->address, portreg->size)); ! physAddr.LowPart = portreg->address; ! physAddr.HighPart = 0; ! if (HalTranslateBusAddress(Isa, 0, physAddr, &memType, &mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_WRITE_PORT\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else ! { ! PVOID logicAddr = (PVOID)mappedAddr.LowPart; ! ntStatus = STATUS_SUCCESS; ! switch (size) { ! case 1: ! KdPrint(("OpenG PORTIO: Value write 0x%X\n", portreg->u.byte)); ! WRITE_PORT_UCHAR(logicAddr, portreg->u.byte); ! break; ! case 2: ! KdPrint(("OpenG PORTIO: Value write 0x%X\n", portreg->u.word)); ! WRITE_PORT_USHORT(logicAddr, portreg->u.word); ! break; ! case 4: ! KdPrint(("OpenG PORTIO: Value write 0x%X\n", portreg->u.dword)); ! WRITE_PORT_ULONG(logicAddr, portreg->u.dword); ! break; ! default: ! KdPrint(("OpenG PORTIO: Invalid port write size\n")); ! ntStatus = STATUS_INVALID_PARAMETER; ! } ! } ! } ! return ntStatus; ! } ! NTSTATUS OGPortIOReadMem(PMEMMAP memmap, ULONG inBufLength, ! PUCHAR pBuffer, ULONG outBufLength, PULONG pOutBufLength) ! { ! NTSTATUS ntStatus = STATUS_BUFFER_TOO_SMALL; ! *pOutBufLength = 0; /* output buffer size */ ! if ((inBufLength >= sizeof(MEMMAP)) && (outBufLength >= memmap->size)) ! { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! PVOID logicAddr; ! ULONG memType = 0, size = memmap->size; ! KdPrint(("OpenG PORTIO: IOCTL_READ_PHYSMEM 0x%X, size %d\n", memmap->address, size)); ! ntStatus = STATUS_SUCCESS; ! physAddr.LowPart = memmap->address; ! physAddr.HighPart = 0; ! ! if (HalTranslateBusAddress(Isa, 0, physAddr, &memType, &mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_READ_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else if (memType == 0) ! { ! logicAddr = MmMapIoSpace(mappedAddr, size, FALSE); ! if (logicAddr == 0) { ! KdPrint(("MmMapIoSpace failed in IOCTL_READ_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; } + } + else + { + logicAddr = (PVOID)mappedAddr.LowPart; + } + if (ntStatus == STATUS_SUCCESS) + { + switch (memmap->unit) + { + case 1: + KdPrint(("OpenG PORTIO: Read memory, unit 1, size %d\n", size)); + READ_REGISTER_BUFFER_UCHAR(logicAddr, (PUCHAR)pBuffer, size); + break; + case 2: + KdPrint(("OpenG PORTIO: Read memory, unit 2, size %d\n", size/2)); + READ_REGISTER_BUFFER_USHORT(logicAddr, (PUSHORT)pBuffer, size/2); + break; + case 4: + KdPrint(("OpenG PORTIO: Read memory, unit 4, size %d\n", size/4)); + READ_REGISTER_BUFFER_ULONG(logicAddr, (PULONG)pBuffer, size/4); + break; + default: + KdPrint(("OpenG PORTIO: Invalid memory read unit %d\n", memmap->unit)); + ntStatus = STATUS_INVALID_PARAMETER; + break; + } if (ntStatus == STATUS_SUCCESS) ! *pOutBufLength = size; /* output buffer size */ ! if (memType == 0) { ! MmUnmapIoSpace(logicAddr, size); ! } ! } ! } ! return ntStatus; ! } ! NTSTATUS OGPortIOWriteMem(PMEMMAP memmap, ULONG inBufLength, PULONG pOutBufLength) ! { ! NTSTATUS ntStatus = STATUS_BUFFER_TOO_SMALL; ! *pOutBufLength = 0; /* output buffer size */ ! if (inBufLength >= sizeof(MEMMAP) + memmap->size) ! { ! PHYSICAL_ADDRESS physAddr; ! PHYSICAL_ADDRESS mappedAddr; ! PVOID logicAddr; ! ULONG memType = 0, size = memmap->size; ! KdPrint(("OpenG PORTIO: IOCTL_WRITE_PHYSMEM 0x%X, size %d\n", memmap->address, size)); ! ntStatus = STATUS_SUCCESS; ! physAddr.LowPart = memmap->address; ! physAddr.HighPart = 0; ! ! if (HalTranslateBusAddress(Isa, 0, physAddr, &memType, &mappedAddr) == FALSE) ! { ! KdPrint(("HalTranslateBusAddress failed in IOCTL_WRITE_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; ! } ! else if (memType == 0) ! { ! logicAddr = MmMapIoSpace(mappedAddr, size, FALSE); ! if (logicAddr == 0) { ! KdPrint(("MmMapIoSpace failed in IOCTL_WRITE_PHYSMEM\n")); ! ntStatus = STATUS_UNSUCCESSFUL; } + } + else + { + logicAddr = (PVOID)mappedAddr.LowPart; + } ! if (ntStatus == STATUS_SUCCESS) ! { ! switch (memmap->unit) { ! case 1: ! KdPrint(("OpenG PORTIO: Write memory, unit 1, size %d\n", size)); ! WRITE_REGISTER_BUFFER_UCHAR(logicAddr, memmap->buf, size); ! break; ! case 2: ! KdPrint(("OpenG PORTIO: Write memory, unit 2, size %d\n", size/2)); ! WRITE_REGISTER_BUFFER_USHORT(logicAddr, (PUSHORT)memmap->buf, size/2); ! break; ! case 4: ! KdPrint(("OpenG PORTIO: Write memory, unit 4, size %d\n", size/4)); ! WRITE_REGISTER_BUFFER_ULONG(logicAddr, (PULONG)memmap->buf, size/4); ! break; ! default: ! KdPrint(("OpenG PORTIO: Invalid memory write unit %d\n", memmap->unit)); ! ntStatus = STATUS_INVALID_PARAMETER; ! break; ! } ! if (memType == 0) ! { ! MmUnmapIoSpace(logicAddr, size); ! } ! } ! } ! return ntStatus; ! } ! NTSTATUS OGPortIOMapMem(BOOLEAN map, PMEMMAP memmap, ULONG inBufLength, PULONG pOutBufLength) ! { ! NTSTATUS ntStatus = STATUS_UNSUCCESSFUL; ! *pOutBufLength = 0; /* output buffer size */ ! return ntStatus; ! } ! NTSTATUS OGPortIODispatch(IN PDEVICE_OBJECT pDevObj, IN PIRP pIrp) ! { ! NTSTATUS ntStatus = STATUS_SUCCESS; ! PUCHAR pIOBuffer = (PUCHAR)pIrp->AssociatedIrp.SystemBuffer; ! PMEMMAP memmap = pIrp->AssociatedIrp.SystemBuffer; ! PIO_STACK_LOCATION irpSp = IoGetCurrentIrpStackLocation(pIrp); ! ULONG inBufLength = irpSp->Parameters.DeviceIoControl.InputBufferLength; ! ULONG outBufLength = irpSp->Parameters.DeviceIoControl.OutputBufferLength; ! switch (irpSp->Parameters.DeviceIoControl.IoControlCode) ! { ! case IOCTL_GET_VERSION: ! KdPrint(("OpenG PORTIO: IOCTL_GET_VERSION\n")); ! ntStatus = OGPortIOVersion((PULONG)pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.OutputBufferLength, ! &pIrp->IoStatus.Information); break; ! case IOCTL_GET_IOPM: ! KdPrint(("OpenG PORTIO: IOCTL_GET_IOPM\n")); ! ntStatus = OGPortIOGetPermMap(pDevObj, pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.OutputBufferLength, ! &pIrp->IoStatus.Information); ! break; ! case IOCTL_SET_IOPM: ! KdPrint(("OpenG PORTIO: IOCTL_SET_IOPM\n")); ! ntStatus = OGPortIOSetPermMap(pDevObj, pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! &pIrp->IoStatus.Information); ! break; ! case IOCTL_RESET_IOPM: ! KdPrint(("OpenG PORTIO: IOCTL_RESET_IOPM\n")); ! ntStatus = OGPortIOProcess(inBufLength >= 4 ? *(PULONG)pIrp->AssociatedIrp.SystemBuffer : 0, FALSE); ! pIrp->IoStatus.Information = 0; /* output buffer size */ ! break; ! case IOCTL_READ_PORT: ! KdPrint(("OpenG PORTIO: IOCTL_READ_PORT\n")); ! ntStatus = OGPortIOReadPort(pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.OutputBufferLength, ! &pIrp->IoStatus.Information); ! break; ! case IOCTL_WRITE_PORT: ! KdPrint(("OpenG PORTIO: IOCTL_WRITE_PORT\n")); ! ntStatus = OGPortIOWritePort(pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! &pIrp->IoStatus.Information); ! break; ! case IOCTL_READ_PHYSMEM: ! KdPrint(("OpenG PORTIO: IOCTL_READ_PHYSMEM\n")); ! ntStatus = OGPortIOReadMem(pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.OutputBufferLength, ! &pIrp->IoStatus.Information); ! break; ! case IOCTL_WRITE_PHYSMEM: ! KdPrint(("OpenG PORTIO: IOCTL_WRITE_PHYSMEM\n")); ! ntStatus = OGPortIOWriteMem(pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! &pIrp->IoStatus.Information); break; ! case IOCTL_MAP_PHYSMEM: ! KdPrint(("OpenG PORTIO: IOCTL_MAP_PHYSMEM\n")); ! ntStatus = OGPortIOMapMem(TRUE, pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! &pIrp->IoStatus.Information); ! break; ! ! case IOCTL_UNMAP_PHYSMEM: ! KdPrint(("OpenG PORTIO: IOCTL_UNMAP_PHYSMEM\n")); ! ntStatus = OGPortIOMapMem(FALSE, pIrp->AssociatedIrp.SystemBuffer, ! irpSp->Parameters.DeviceIoControl.InputBufferLength, ! &pIrp->IoStatus.Information); ! break; ! ! default: ! KdPrint(("OpenG PORTIO: Unsupported IOCTL Call 0x%x\n", irpSp->Parameters.DeviceIoControl.IoControlCode)); ntStatus = STATUS_UNSUCCESSFUL; pIrp->IoStatus.Information = 0; |