moel@347: //-----------------------------------------------------------------------------
moel@347: //     Author : hiyohiyo
moel@347: //       Mail : hiyohiyo@crystalmark.info
moel@347: //        Web : http://openlibsys.org/
moel@347: //    License : The modified BSD license
moel@347: //
moel@347: //                     Copyright 2007-2008 OpenLibSys.org. All rights reserved.
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: #include <ntddk.h>
moel@347: #include <stddef.h>
moel@347: #include "OpenLibSys.h"
moel@347: 
moel@347: //-----------------------------------------------------------------------------
moel@347: //
moel@347: // Global
moel@347: //
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: static ULONG refCount;
moel@347: 
moel@347: //-----------------------------------------------------------------------------
moel@347: //
moel@347: // Classic NT driver
moel@347: // DriverEntry / OlsDispatch / Unload
moel@347: //
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: NTSTATUS
moel@347: DriverEntry(
moel@347: 	IN PDRIVER_OBJECT  DriverObject,
moel@347: 	IN PUNICODE_STRING RegistryPath
moel@347: 	)
moel@347: 
moel@347: /*
moel@347: 
moel@347: Return Value:
moel@347: 
moel@347: 	STATUS_SUCCESS if the driver initialized correctly, otherwise an erroror
moel@347: 	indicating the reason for failure.
moel@347: */
moel@347: 
moel@347: {
moel@347: 	NTSTATUS		status;
moel@347: 	UNICODE_STRING  ntDeviceName;
moel@347: 	UNICODE_STRING  win32DeviceName;
moel@347: 	PDEVICE_OBJECT  deviceObject = NULL;
moel@347: 
moel@347: 	RtlInitUnicodeString(&ntDeviceName, NT_DEVICE_NAME);
moel@347: 
moel@347: 	status = IoCreateDevice(
moel@347: 		DriverObject,					// Our Driver Object
moel@347: 		0,								// We don't use a device extension
moel@347: 		&ntDeviceName,					// Device name 
moel@347: 		OLS_TYPE,						// Device type
moel@347: 		FILE_DEVICE_SECURE_OPEN,		// Device characteristics
moel@347: 		FALSE,							// Not an exclusive device
moel@347: 		&deviceObject );				// Returned ptr to Device Object
moel@347: 
moel@347: 	if(!NT_SUCCESS(status))
moel@347: 	{
moel@347: 		refCount = (ULONG)-1;
moel@347: 		return status;
moel@347: 	}
moel@347: 	else
moel@347: 	{
moel@347: 		refCount = 0;
moel@347: 	}
moel@347: 
moel@347: 	// Initialize the driver object with this driver's entry points.
moel@347: 	DriverObject->MajorFunction[IRP_MJ_CREATE] = OlsDispatch;
moel@347: 	DriverObject->MajorFunction[IRP_MJ_CLOSE] = OlsDispatch;
moel@347: 	DriverObject->MajorFunction[IRP_MJ_DEVICE_CONTROL] = OlsDispatch;
moel@347: 	DriverObject->DriverUnload = Unload;
moel@347: 
moel@347: 	// Initialize a Unicode String containing the Win32 name for our device.
moel@347: 	RtlInitUnicodeString(&win32DeviceName, DOS_DEVICE_NAME);
moel@347: 
moel@347: 	// Create a symbolic link between our device name  and the Win32 name
moel@347: 	status = IoCreateSymbolicLink(&win32DeviceName, &ntDeviceName);
moel@347: 
moel@347: 	if (!NT_SUCCESS(status))
moel@347: 	{
moel@347: 		// Delete everything that this routine has allocated.
moel@347: 		IoDeleteDevice( deviceObject );
moel@347: 	}
moel@347: 
moel@347: 	return status;
moel@347: }
moel@347: 
moel@347: NTSTATUS
moel@347: OlsDispatch(
moel@347: 	IN	PDEVICE_OBJECT pDO,
moel@347: 	IN	PIRP pIrp
moel@347: 	)
moel@347: 
moel@347: /*++
moel@347: 
moel@347: Routine Description:
moel@347: 	This routine is the dispatch handler for the driver.  It is responsible
moel@347: 	for processing the IRPs.
moel@347: 
moel@347: Arguments:
moel@347: 	
moel@347: 	pDO - Pointer to device object.
moel@347: 
moel@347: 	pIrp - Pointer to the current IRP.
moel@347: 
moel@347: Return Value:
moel@347: 
moel@347: 	STATUS_SUCCESS if the IRP was processed successfully, otherwise an erroror
moel@347: 	indicating the reason for failure.
moel@347: 
moel@347: --*/
moel@347: 
moel@347: {
moel@347: 	PIO_STACK_LOCATION pIrpStack;
moel@347: 	NTSTATUS status;
moel@347: 	int index;
moel@347: 
moel@347: 	//  Initialize the irp info field.
moel@347: 	//	  This is used to return the number of bytes transfered.
moel@347: 	pIrp->IoStatus.Information = 0;
moel@347: 	pIrpStack = IoGetCurrentIrpStackLocation(pIrp);
moel@347: 
moel@347: 	//  Set default return status
moel@347: 	status = STATUS_NOT_IMPLEMENTED;
moel@347: 
moel@347: 	// Dispatch based on major fcn code.
moel@347: 	switch(pIrpStack->MajorFunction)
moel@347: 	{
moel@347: 		case IRP_MJ_CREATE:
moel@347: 			if(refCount != (ULONG)-1){refCount++;}
moel@347: 			status = STATUS_SUCCESS;
moel@347: 			break;
moel@347: 		case IRP_MJ_CLOSE:
moel@347: 			if(refCount != (ULONG)-1){refCount--;}
moel@347: 			status = STATUS_SUCCESS;
moel@347: 			break;
moel@347: 
moel@347: 		case IRP_MJ_DEVICE_CONTROL:
moel@347: 			//  Dispatch on IOCTL
moel@347: 			switch(pIrpStack->Parameters.DeviceIoControl.IoControlCode)
moel@347: 			{
moel@347: 			case IOCTL_OLS_GET_DRIVER_VERSION:
moel@347: 				*(PULONG)pIrp->AssociatedIrp.SystemBuffer = OLS_DRIVER_VERSION;
moel@347: 				pIrp->IoStatus.Information = 4;
moel@347: 				status = STATUS_SUCCESS;
moel@347: 				break;
moel@347: 
moel@347: 			case IOCTL_OLS_GET_REFCOUNT:
moel@347: 				*(PULONG)pIrp->AssociatedIrp.SystemBuffer = refCount;
moel@347: 				pIrp->IoStatus.Information = sizeof(refCount);
moel@347: 				status = STATUS_SUCCESS;
moel@347: 				break;
moel@347: 
moel@347: 			case IOCTL_OLS_READ_MSR:
moel@347: 				status = ReadMsr(
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 			case IOCTL_OLS_WRITE_MSR:
moel@347: 				status = WriteMsr(
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 			case IOCTL_OLS_READ_PMC:
moel@347: 				status = ReadPmc(
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 			case IOCTL_OLS_HALT:
moel@347: 				__halt();
moel@347: 				status = STATUS_SUCCESS;
moel@347: 				break;
moel@347: 
moel@347: 			case IOCTL_OLS_READ_IO_PORT:
moel@347: 			case IOCTL_OLS_READ_IO_PORT_BYTE:
moel@347: 			case IOCTL_OLS_READ_IO_PORT_WORD:
moel@347: 			case IOCTL_OLS_READ_IO_PORT_DWORD:
moel@347: 				status = ReadIoPort(
moel@347: 					pIrpStack->Parameters.DeviceIoControl.IoControlCode,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 			case IOCTL_OLS_WRITE_IO_PORT:
moel@347: 			case IOCTL_OLS_WRITE_IO_PORT_BYTE:
moel@347: 			case IOCTL_OLS_WRITE_IO_PORT_WORD:
moel@347: 			case IOCTL_OLS_WRITE_IO_PORT_DWORD:
moel@347: 				status = WriteIoPort(
moel@347: 					pIrpStack->Parameters.DeviceIoControl.IoControlCode,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 
moel@347: 			case IOCTL_OLS_READ_PCI_CONFIG:
moel@347: 				status = ReadPciConfig(
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 			case IOCTL_OLS_WRITE_PCI_CONFIG:
moel@347: 				status = WritePciConfig(
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 
moel@347: 			case IOCTL_OLS_READ_MEMORY:
moel@347: 				status = ReadMemory(
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 			case IOCTL_OLS_WRITE_MEMORY:
moel@347: 				status = WriteMemory(
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.InputBufferLength,
moel@347: 					pIrp->AssociatedIrp.SystemBuffer,
moel@347: 					pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
moel@347: 					(ULONG*)&pIrp->IoStatus.Information
moel@347: 					);
moel@347: 				break;
moel@347: 
moel@347: 
moel@347: 			}
moel@347: 			break;
moel@347: 	}
moel@347: 
moel@347: 	// We're done with I/O request.  Record the status of the I/O action.
moel@347: 	pIrp->IoStatus.Status = status;
moel@347: 
moel@347: 	// Don't boost priority when returning since this took little time.
moel@347: 	IoCompleteRequest(pIrp, IO_NO_INCREMENT);
moel@347: 
moel@347: 	return status;
moel@347: }
moel@347: 
moel@347: VOID
moel@347: Unload(
moel@347: 	PDRIVER_OBJECT DriverObject
moel@347: 	)
moel@347: /*++
moel@347: 
moel@347: Routine Description:
moel@347: 
moel@347: 	This routine is called by the I/O system to unload the driver.
moel@347: 
moel@347: 	Any resources previously allocated must be freed.
moel@347: 
moel@347: Arguments:
moel@347: 
moel@347: 	DriverObject - a pointer to the object that represents our driver.
moel@347: 
moel@347: Return Value:
moel@347: 
moel@347: 	None
moel@347: --*/
moel@347: 
moel@347: {
moel@347: 	PDEVICE_OBJECT deviceObject = DriverObject->DeviceObject;
moel@347: 	UNICODE_STRING win32NameString;
moel@347: 
moel@347: 	PAGED_CODE();
moel@347: 
moel@347: 	// Create counted string version of our Win32 device name.
moel@347: 	RtlInitUnicodeString(&win32NameString, DOS_DEVICE_NAME);
moel@347: 
moel@347: 	// Delete the link from our device name to a name in the Win32 namespace.
moel@347: 	IoDeleteSymbolicLink(&win32NameString);
moel@347: 
moel@347: 	if(deviceObject != NULL)
moel@347: 	{
moel@347: 		IoDeleteDevice(deviceObject);
moel@347: 	}
moel@347: }
moel@347: 
moel@347: //-----------------------------------------------------------------------------
moel@347: //
moel@347: // CPU
moel@347: //
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: NTSTATUS
moel@347: ReadMsr(	void	*lpInBuffer, 
moel@347: 			ULONG	nInBufferSize, 
moel@347: 			void	*lpOutBuffer, 
moel@347: 			ULONG	nOutBufferSize, 
moel@347: 			ULONG	*lpBytesReturned)
moel@347: {
moel@347: 	__try
moel@347: 	{
moel@347: 		ULONGLONG data = __readmsr(*(ULONG*)lpInBuffer);
moel@347: 		memcpy((PULONG)lpOutBuffer, &data, 8);
moel@347: 		*lpBytesReturned = 8;
moel@347: 		return STATUS_SUCCESS;
moel@347: 	}
moel@347: 	__except(EXCEPTION_EXECUTE_HANDLER)
moel@347: 	{
moel@347: 		*lpBytesReturned = 0;
moel@347: 		return STATUS_UNSUCCESSFUL;
moel@347: 	}
moel@347: }
moel@347: 
moel@347: NTSTATUS
moel@347: WriteMsr(	void	*lpInBuffer, 
moel@347: 			ULONG	nInBufferSize, 
moel@347: 			void	*lpOutBuffer, 
moel@347: 			ULONG	nOutBufferSize, 
moel@347: 			ULONG	*lpBytesReturned)
moel@347: {
moel@347: 	__try
moel@347: 	{
moel@347: 		OLS_WRITE_MSR_INPUT* param;
moel@347: 		param = (OLS_WRITE_MSR_INPUT*)lpInBuffer;
moel@347: 
moel@347: 		__writemsr(param->Register, param->Value.QuadPart);
moel@347: 		*lpBytesReturned = 0;
moel@347: 		return STATUS_SUCCESS;
moel@347: 	}
moel@347: 	__except(EXCEPTION_EXECUTE_HANDLER)
moel@347: 	{
moel@347: 		*lpBytesReturned = 0;
moel@347: 		return STATUS_UNSUCCESSFUL;
moel@347: 	}
moel@347: }
moel@347: 
moel@347: NTSTATUS
moel@347: ReadPmc(	void	*lpInBuffer, 
moel@347: 			ULONG	nInBufferSize, 
moel@347: 			void	*lpOutBuffer, 
moel@347: 			ULONG	nOutBufferSize, 
moel@347: 			ULONG	*lpBytesReturned)
moel@347: {
moel@347: 	__try
moel@347: 	{
moel@347: 		ULONGLONG data = __readpmc(*(ULONG*)lpInBuffer);
moel@347: 		memcpy((PULONG)lpOutBuffer, &data, 8);
moel@347: 		*lpBytesReturned = 8;
moel@347: 		return STATUS_SUCCESS;
moel@347: 	}
moel@347: 	__except(EXCEPTION_EXECUTE_HANDLER)
moel@347: 	{
moel@347: 		*lpBytesReturned = 0;
moel@347: 		return STATUS_UNSUCCESSFUL;
moel@347: 	}
moel@347: }
moel@347: 
moel@347: //-----------------------------------------------------------------------------
moel@347: //
moel@347: // IO Port
moel@347: //
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: NTSTATUS
moel@347: ReadIoPort( ULONG	ioControlCode,
moel@347: 			void	*lpInBuffer, 
moel@347: 			ULONG	nInBufferSize, 
moel@347: 			void	*lpOutBuffer, 
moel@347: 			ULONG	nOutBufferSize, 
moel@347: 			ULONG	*lpBytesReturned)
moel@347: {
moel@347: 	ULONG nPort = *(ULONG*)lpInBuffer;
moel@347: 
moel@347: 	switch(ioControlCode)
moel@347: 	{
moel@347: 		case IOCTL_OLS_READ_IO_PORT_BYTE:
moel@347: 			*(PUCHAR)lpOutBuffer = READ_PORT_UCHAR((PUCHAR)(ULONG_PTR)nPort);
moel@347: 			break;
moel@347: 		case IOCTL_OLS_READ_IO_PORT_WORD:
moel@347: 			*(PUSHORT)lpOutBuffer = READ_PORT_USHORT((PUSHORT)(ULONG_PTR)nPort);
moel@347: 			break;
moel@347: 		case IOCTL_OLS_READ_IO_PORT_DWORD:
moel@347: 			*(PULONG)lpOutBuffer = READ_PORT_ULONG((PULONG)(ULONG_PTR)nPort);
moel@347: 			break;
moel@347: 		default:
moel@347: 			*lpBytesReturned = 0;
moel@347: 			return STATUS_INVALID_PARAMETER;
moel@347: 			break;
moel@347: 	}
moel@347: 	
moel@347: 	*lpBytesReturned = nInBufferSize;
moel@347: 	return STATUS_SUCCESS;
moel@347: }
moel@347: 
moel@347: NTSTATUS
moel@347: WriteIoPort(ULONG	ioControlCode,
moel@347: 			void	*lpInBuffer, 
moel@347: 			ULONG	nInBufferSize, 
moel@347: 			void	*lpOutBuffer, 
moel@347: 			ULONG	nOutBufferSize, 
moel@347: 			ULONG	*lpBytesReturned)
moel@347: {
moel@347: 	ULONG nPort;
moel@347: 	OLS_WRITE_IO_PORT_INPUT* param;
moel@347: 	
moel@347: 	param = (OLS_WRITE_IO_PORT_INPUT*)lpInBuffer;
moel@347: 	nPort = param->PortNumber;
moel@347: 
moel@347: 	switch(ioControlCode)
moel@347: 	{
moel@347: 
moel@347: 		case IOCTL_OLS_WRITE_IO_PORT_BYTE:
moel@347: 			WRITE_PORT_UCHAR((PUCHAR)(ULONG_PTR)nPort, param->CharData);
moel@347: 			break;
moel@347: 		case IOCTL_OLS_WRITE_IO_PORT_WORD:
moel@347: 			WRITE_PORT_USHORT((PUSHORT)(ULONG_PTR)nPort, param->ShortData);
moel@347: 			break;
moel@347: 		case IOCTL_OLS_WRITE_IO_PORT_DWORD:
moel@347: 			WRITE_PORT_ULONG((PULONG)(ULONG_PTR)nPort, param->LongData);
moel@347: 			break;
moel@347: 		default:
moel@347: 			return STATUS_INVALID_PARAMETER;
moel@347: 			break;
moel@347: 	}
moel@347: 
moel@347: 	return STATUS_SUCCESS;
moel@347: }
moel@347: 
moel@347: //-----------------------------------------------------------------------------
moel@347: //
moel@347: // PCI
moel@347: //
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: NTSTATUS
moel@347: ReadPciConfig(	void	*lpInBuffer, 
moel@347: 				ULONG	nInBufferSize, 
moel@347: 				void	*lpOutBuffer, 
moel@347: 				ULONG	nOutBufferSize, 
moel@347: 				ULONG	*lpBytesReturned)
moel@347: {
moel@347: 	OLS_READ_PCI_CONFIG_INPUT *param;
moel@347: 	NTSTATUS status;
moel@347: 
moel@347: 	if(nInBufferSize != sizeof(OLS_READ_PCI_CONFIG_INPUT))
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 	param = (OLS_READ_PCI_CONFIG_INPUT *)lpInBuffer;
moel@347: 
moel@347: 	status = pciConfigRead(param->PciAddress, param->PciOffset,
moel@347: 						lpOutBuffer, nOutBufferSize);
moel@347: 
moel@347: 	if(status == STATUS_SUCCESS)
moel@347: 	{
moel@347: 		*lpBytesReturned = nOutBufferSize;
moel@347: 	}
moel@347: 	else
moel@347: 	{
moel@347: 		*lpBytesReturned = 0;
moel@347: 	}
moel@347: 
moel@347: 	return status;
moel@347: }
moel@347: 
moel@347: NTSTATUS
moel@347: WritePciConfig(	void *lpInBuffer, 
moel@347: 				ULONG nInBufferSize, 
moel@347: 				void *lpOutBuffer, 
moel@347: 				ULONG nOutBufferSize, 
moel@347: 				ULONG *lpBytesReturned)
moel@347: 
moel@347: {
moel@347: 	OLS_WRITE_PCI_CONFIG_INPUT *param;
moel@347: 	ULONG writeSize;
moel@347: 	NTSTATUS status;
moel@347: 
moel@347: 	if(nInBufferSize < offsetof(OLS_WRITE_PCI_CONFIG_INPUT, Data))
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: 	param = (OLS_WRITE_PCI_CONFIG_INPUT *)lpInBuffer;
moel@347: 	writeSize = nInBufferSize - offsetof(OLS_WRITE_PCI_CONFIG_INPUT, Data);
moel@347: 	
moel@347: 	*lpBytesReturned = 0;
moel@347: 
moel@347: 	return pciConfigWrite(param->PciAddress, param->PciOffset,
moel@347: 							&param->Data, writeSize);
moel@347: 
moel@347: }
moel@347: 
moel@347: //-----------------------------------------------------------------------------
moel@347: //
moel@347: // Support Function
moel@347: //
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: NTSTATUS pciConfigRead(ULONG pciAddress, ULONG offset, void *data, int length)
moel@347: {
moel@347: 	PCI_SLOT_NUMBER slot;
moel@347: 	int error;
moel@347: 	ULONG busNumber;
moel@347: 
moel@347: 	busNumber = PciGetBus(pciAddress);
moel@347: 	slot.u.AsULONG = 0;
moel@347: 	slot.u.bits.DeviceNumber = PciGetDev(pciAddress);
moel@347: 	slot.u.bits.FunctionNumber = PciGetFunc(pciAddress);
moel@347: 	error =	HalGetBusDataByOffset(PCIConfiguration, busNumber, slot.u.AsULONG,
moel@347: 									data, offset, length);
moel@347: 
moel@347: 	if(error == 0)
moel@347: 	{
moel@347: 		return OLS_ERROR_PCI_BUS_NOT_EXIST;
moel@347: 	}
moel@347: 	else if(length != 2 && error == 2)
moel@347: 	{
moel@347: 		return OLS_ERROR_PCI_NO_DEVICE;
moel@347: 	}
moel@347: 	else if(length != error)
moel@347: 	{
moel@347: 		return OLS_ERROR_PCI_READ_CONFIG;
moel@347: 	}
moel@347: 
moel@347: 	return STATUS_SUCCESS;
moel@347: }
moel@347: 
moel@347: NTSTATUS pciConfigWrite(ULONG pciAddress, ULONG offset, void *data, int length)
moel@347: {
moel@347: 	PCI_SLOT_NUMBER slot;
moel@347: 	int error;
moel@347: 	ULONG busNumber;
moel@347: 
moel@347: 	busNumber = PciGetBus(pciAddress);
moel@347: 
moel@347: 	slot.u.AsULONG = 0;
moel@347: 	slot.u.bits.DeviceNumber = PciGetDev(pciAddress);
moel@347: 	slot.u.bits.FunctionNumber = PciGetFunc(pciAddress);
moel@347: 	error = HalSetBusDataByOffset(PCIConfiguration, busNumber, slot.u.AsULONG,
moel@347: 									data, offset, length);
moel@347: 
moel@347: 	if(error != length)
moel@347: 	{
moel@347: 		return OLS_ERROR_PCI_WRITE_CONFIG;
moel@347: 	}
moel@347: 
moel@347: 	return STATUS_SUCCESS;
moel@347: }
moel@347: 
moel@347: 
moel@347: //-----------------------------------------------------------------------------
moel@347: //
moel@347: // Physical Memory
moel@347: //
moel@347: //-----------------------------------------------------------------------------
moel@347: 
moel@347: NTSTATUS
moel@347: ReadMemory(	void	*lpInBuffer, 
moel@347: 			ULONG	nInBufferSize, 
moel@347: 			void	*lpOutBuffer, 
moel@347: 			ULONG	nOutBufferSize, 
moel@347: 			ULONG	*lpBytesReturned)
moel@347: {
moel@347: 	OLS_READ_MEMORY_INPUT *param;
moel@347: 	ULONG	size;
moel@347: 	PHYSICAL_ADDRESS address;
moel@347: 	PVOID	maped;
moel@347: 	BOOLEAN	error;
moel@347: 
moel@347: 	if(nInBufferSize != sizeof(OLS_READ_MEMORY_INPUT))
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: 	param = (OLS_READ_MEMORY_INPUT *)lpInBuffer;
moel@347: 	size = param->UnitSize * param->Count;
moel@347: 
moel@347: 	if(nOutBufferSize < size)
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: 	address.QuadPart = param->Address.QuadPart;
moel@347: 
moel@347: #ifndef _PHYSICAL_MEMORY_SUPPORT
moel@347: 
moel@347: 	if(0x000C0000 > address.QuadPart 
moel@347: 	|| (address.QuadPart + size - 1) > 0x000FFFFF)
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: #endif
moel@347: 
moel@347: 	maped = MmMapIoSpace(address, size, FALSE);
moel@347: 
moel@347: 	error = FALSE;
moel@347: 	switch(param->UnitSize){
moel@347: 		case 1:
moel@347: 			READ_REGISTER_BUFFER_UCHAR(maped, lpOutBuffer, param->Count);
moel@347: 			break;
moel@347: 		case 2:
moel@347: 			READ_REGISTER_BUFFER_USHORT(maped, lpOutBuffer, param->Count);
moel@347: 			break;
moel@347: 		case 4:
moel@347: 			READ_REGISTER_BUFFER_ULONG(maped, lpOutBuffer, param->Count);
moel@347: 			break;
moel@347: 		default:
moel@347: 			error = TRUE;
moel@347: 			break;
moel@347: 	}
moel@347: 
moel@347: 	MmUnmapIoSpace(maped, size);
moel@347: 
moel@347: 	if(error)
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: 	*lpBytesReturned = nOutBufferSize;
moel@347: 
moel@347: 	return STATUS_SUCCESS;
moel@347: }
moel@347: 
moel@347: NTSTATUS
moel@347: WriteMemory(void	*lpInBuffer, 
moel@347: 			ULONG	nInBufferSize, 
moel@347: 			void	*lpOutBuffer, 
moel@347: 			ULONG	nOutBufferSize, 
moel@347: 			ULONG	*lpBytesReturned)
moel@347: {
moel@347: #ifdef _PHYSICAL_MEMORY_SUPPORT
moel@347: 
moel@347: 	OLS_WRITE_MEMORY_INPUT *param;
moel@347: 	ULONG size;
moel@347: 	PHYSICAL_ADDRESS address;
moel@347: 	PVOID	maped;
moel@347: 	BOOLEAN	error;
moel@347: 
moel@347: 	if(nInBufferSize < offsetof(OLS_WRITE_MEMORY_INPUT, Data))
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: 	param = (OLS_WRITE_MEMORY_INPUT *)lpInBuffer;
moel@347: 
moel@347: 	size = param->UnitSize * param->Count;
moel@347: 	if (nInBufferSize < size + offsetof(OLS_WRITE_MEMORY_INPUT, Data))
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: 	address.QuadPart = param->Address.QuadPart;
moel@347: 
moel@347: 	maped = MmMapIoSpace(address, size, FALSE);
moel@347: 
moel@347: 	error = FALSE;
moel@347: 	switch(param->UnitSize){
moel@347: 		case 1:
moel@347: 			WRITE_REGISTER_BUFFER_UCHAR(maped, 
moel@347: 										(UCHAR*)&param->Data, param->Count);
moel@347: 			break;
moel@347: 		case 2:
moel@347: 			WRITE_REGISTER_BUFFER_USHORT(maped,
moel@347: 										(USHORT*)&param->Data, param->Count);
moel@347: 			break;
moel@347: 		case 4:
moel@347: 			WRITE_REGISTER_BUFFER_ULONG(maped,
moel@347: 										(ULONG*)&param->Data, param->Count);
moel@347: 			break;
moel@347: 		default:
moel@347: 			error = TRUE;
moel@347: 			break;
moel@347: 	}
moel@347: 
moel@347: 	MmUnmapIoSpace(maped, size);
moel@347: 
moel@347: 	if(error)
moel@347: 	{
moel@347: 		return STATUS_INVALID_PARAMETER;
moel@347: 	}
moel@347: 
moel@347: 	*lpBytesReturned = 0;
moel@347: 
moel@347: 	return STATUS_SUCCESS;
moel@347: 
moel@347: #else
moel@347: 
moel@347: 	*lpBytesReturned = 0;
moel@347: 	
moel@347: 	return STATUS_INVALID_PARAMETER;
moel@347: 
moel@347: #endif
moel@347: }