[opengtoolkit-cvs] portIO/c_source ogportio.c,1.3,1.4

[Rolf K. <lab...@us...>]

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;