Драйвер манипулирования лед лампочек на клавиатуре c++

#include <ntddk.h>
 
// status register bits
#define IBUFFER_FULL 0x02
#define OBUFFER_FULL 0x01
 
#define IOCTL_CODE1 CTL_CODE(\
    FILE_DEVICE_UNKNOWN, 0x801, METHOD_BUFFERED, FILE_ANY_ACCESS)
 
#define IOCTL_CODE2 CTL_CODE(\
    FILE_DEVICE_UNKNOWN, 0x802, METHOD_BUFFERED, FILE_ANY_ACCESS)
 
#define IOCTL_SCROLLLOCK_ON CTL_CODE(\
    FILE_DEVICE_UNKNOWN, 0x803, METHOD_BUFFERED, FILE_ANY_ACCESS)
 
#define IOCTL_SCROLLLOCK_OFF CTL_CODE(\
    FILE_DEVICE_UNKNOWN, 0x804, METHOD_BUFFERED, FILE_ANY_ACCESS)
 
NTSTATUS DriverEntry(IN PDRIVER_OBJECT DriverObject, IN PUNICODE_STRING RegistryPath);
VOID UnloadRoutine(IN PDRIVER_OBJECT DriverObject);
 
// Предварительные объявления функций:
NTSTATUS CompleteIrp(PIRP Irp, NTSTATUS status, ULONG info);
NTSTATUS CreateFile_IRPprocessing(IN PDEVICE_OBJECT fdo, IN PIRP Irp);
NTSTATUS CloseHandle_IRPprocessing(IN PDEVICE_OBJECT fdo, IN PIRP Irp);
NTSTATUS Write_IRPhandler(IN PDEVICE_OBJECT fdo, IN PIRP Irp);
NTSTATUS Read_IRPhandler(IN PDEVICE_OBJECT fdo, IN PIRP Irp);
//NTSTATUS ReadWrite_IRPhandler(IN PDEVICE_OBJECT fdo, IN PIRP Irp);
NTSTATUS DeviceControlRoutine(IN PDEVICE_OBJECT fdo, IN PIRP Irp);
 
 
//#pragma alloc_text(INIT, DriverEntry)
//#pragma alloc_text(PAGE, UnloadRoutine)
 
 
int CheckKeyboardBuffer() {
  UCHAR state;
  int i = 0;
 
  for(i = 0; i<100; i++) {
 
    state = READ_PORT_UCHAR((PUCHAR)0x64);
    KeStallExecutionProcessor(665);
 
    if(!(state & IBUFFER_FULL)) {
      return TRUE;
    }       
  }
 
  return FALSE;
 
}
 
 
NTSTATUS DriverEntry(IN PDRIVER_OBJECT DriverObject, IN PUNICODE_STRING RegistryPath)
{
  UNICODE_STRING usDeviceNameUnicodeString;
  UNICODE_STRING usDeviceLinkUnicodeString;
 
  PDEVICE_OBJECT DeviceObject = NULL;
  NTSTATUS Res;
 
 
  DriverObject->DriverUnload = UnloadRoutine;
 
  DbgPrint("TestDriver begin started!\n");
  DbgPrint("IOCTL_CODE1 = %x\n", IOCTL_CODE1);
  DbgPrint("IOCTL_CODE2 = %x\n", IOCTL_CODE2);
  DbgPrint("IOCTL_SCROLLLOCK_ON = %x\n", IOCTL_SCROLLLOCK_ON);
  DbgPrint("IOCTL_SCROLLLOCK_OFF = %x\n", IOCTL_SCROLLLOCK_OFF);
 
  DriverObject->MajorFunction[IRP_MJ_CREATE]= CreateFile_IRPprocessing;
  DriverObject->MajorFunction[IRP_MJ_CLOSE] = CloseHandle_IRPprocessing;
  DriverObject->MajorFunction[IRP_MJ_WRITE] = Write_IRPhandler;
  DriverObject->MajorFunction[IRP_MJ_READ]  = Read_IRPhandler;
    
  // вариант с одной подрограммой
  //DriverObject->MajorFunction[IRP_MJ_READ]  = ReadWrite_IRPhandler;
  //DriverObject->MajorFunction[IRP_MJ_WRITE] = ReadWrite_IRPhandler;
  
  DriverObject->MajorFunction[IRP_MJ_DEVICE_CONTROL]= DeviceControlRoutine;
 
  // Подготовка Unicode-строк
  // не работает RtlInitUnicodeString (&usDeviceNameUnicodeString, L"\\Device\\TestDevice");
  RtlInitUnicodeString (&usDeviceNameUnicodeString, L"\\Device\\TestDriver");
  
  // Создание устройства
  Res = IoCreateDevice(DriverObject, 
                       sizeof(PDEVICE_OBJECT),
                       &usDeviceNameUnicodeString,
                       FILE_DEVICE_UNKNOWN,
                       0,
                       FALSE,
                       &DeviceObject);
 
  // Выход в случае ошибки при создании устройства
  if (!NT_SUCCESS(Res)) {
    DbgPrint("ERROR: Device not created!\n");
    return STATUS_UNSUCCESSFUL;
  }
 
  // не работает RtlInitUnicodeString (&usDeviceLinkUnicodeString, L"\\??\\TestDevice");  
  RtlInitUnicodeString (&usDeviceLinkUnicodeString, L"\\DosDevices\\TestDriver");
  
 
 
  // Создание символьной ссылки 
  Res = IoCreateSymbolicLink (&usDeviceLinkUnicodeString, &usDeviceNameUnicodeString);
  if (!NT_SUCCESS(Res)) {
    IoDeleteDevice(DeviceObject);
    DbgPrint("ERROR: Symbolic link not created!\n");
    return STATUS_UNSUCCESSFUL;
  }
  
  DbgPrint("TestDriver allready started!\n");
 
  return STATUS_SUCCESS;
}
 
VOID UnloadRoutine(IN PDRIVER_OBJECT DriverObject)
{
  UNICODE_STRING deviceLink;
  RtlInitUnicodeString(&deviceLink, L"\\Device\\TestDriver");
  IoDeleteSymbolicLink(&deviceLink);
  IoDeleteDevice(DriverObject->DeviceObject);  
 
  DbgPrint("TestDriver stopped!\n");
}
 
 
 
NTSTATUS CompleteIrp(PIRP Irp, NTSTATUS status, ULONG info)
{
  Irp->IoStatus.Status = status;
  Irp->IoStatus.Information = info;
  IoCompleteRequest(Irp,IO_NO_INCREMENT);return status;
}
 
 
NTSTATUS CreateFile_IRPprocessing(IN PDEVICE_OBJECT fdo,IN PIRP Irp)
{
  //PIO_STACK_LOCATION IrpStack = IoGetCurrentIrpStackLocation(Irp);
    
  DbgPrint("CreateFile processing!\n");
 
  return CompleteIrp(Irp,STATUS_SUCCESS,0); // Успешное завершение
}
 
 
NTSTATUS CloseHandle_IRPprocessing(IN PDEVICE_OBJECT fdo,IN PIRP Irp)
{
  //PIO_STACK_LOCATION IrpStack = IoGetCurrentIrpStackLocation(Irp);
    
  DbgPrint("CloseHandle processing!\n");
 
  return CompleteIrp(Irp,STATUS_SUCCESS,0); // Успешное завершение
}
 
 
 
NTSTATUS Write_IRPhandler(IN PDEVICE_OBJECT fdo, IN PIRP Irp)
{
  //PIO_STACK_LOCATION IrpStack = IoGetCurrentIrpStackLocation(Irp);
    
  DbgPrint("Write processing!\n");
 
  return CompleteIrp(Irp,STATUS_SUCCESS,0); // Успешное завершение
}
 
 
 
NTSTATUS Read_IRPhandler(IN PDEVICE_OBJECT fdo, IN PIRP Irp)
{
  //PIO_STACK_LOCATION IrpStack = IoGetCurrentIrpStackLocation(Irp);
    
  DbgPrint("Read processing!\n");
 
  return CompleteIrp(Irp,STATUS_SUCCESS,0); // Успешное завершение
}
 
 
 
NTSTATUS DeviceControlRoutine(IN PDEVICE_OBJECT fdo, IN PIRP Irp) 
{
  
  NTSTATUS Res = STATUS_SUCCESS;
  PVOID pIOBuffer;       // буфер для обмена с приложением 
 
  PIO_STACK_LOCATION pIrpStack;
 
  pIrpStack = IoGetCurrentIrpStackLocation(Irp);
  pIOBuffer = Irp->AssociatedIrp.SystemBuffer;
 
  DbgPrint("DeviceControl Processing...\n");
 
  // выбор кода операции
  switch (pIrpStack->Parameters.DeviceIoControl.IoControlCode) {
    
    case IOCTL_CODE1:
      DbgPrint("Processing IOCTL_CODE1...\n");
      break;
 
    case IOCTL_CODE2:
      DbgPrint("Processing IOCTL_CODE2...\n");
      break;
 
    case IOCTL_SCROLLLOCK_ON:
      DbgPrint("Processing IOCTL_SCROLLLOCK_ON...\n");
  
      if(CheckKeyboardBuffer()) {
        WRITE_PORT_UCHAR((PUCHAR)0x60,0xED);
      WRITE_PORT_UCHAR((PUCHAR)0x60,1);
      }
 
      break;
 
    case IOCTL_SCROLLLOCK_OFF:
      DbgPrint("Processing IOCTL_SCROLLLOCK_OFF...\n");
  
      if(CheckKeyboardBuffer()) {
        WRITE_PORT_UCHAR((PUCHAR)0x60,0xED);
      WRITE_PORT_UCHAR((PUCHAR)0x60,0);
      }
 
      break;     
  }
 
  return CompleteIrp(Irp,STATUS_SUCCESS,0); // Успешное завершение
 
};