adddev.c

这个是一个开源项目, 有能力的人可以一起来写

    SysLog(pPhDevObj, status, L"AddFdoPort StrAppendStr0 FAIL");
  }

  status = IoRegisterDeviceInterface(pPhDevObj,
                                     (LPGUID)&GUID_CLASS_COMPORT,
                                     NULL,
                                     &pDevExt->symbolicLinkName);

  if (NT_SUCCESS(status)) {
    status = IoSetDeviceInterfaceState(&pDevExt->symbolicLinkName, TRUE);

    if (!NT_SUCCESS(status))
      SysLog(pPhDevObj, status, L"AddFdoPort IoSetDeviceInterfaceState FAIL");
  } else {
    SysLog(pPhDevObj, status, L"AddFdoPort IoRegisterDeviceInterface FAIL");
    pDevExt->symbolicLinkName.Buffer = NULL;
  }

  status = IoWMIRegistrationControl(pNewDevObj, WMIREG_ACTION_REGISTER);

  if (!NT_SUCCESS(status))
    SysLog(pPhDevObj, status, L"AddFdoPort IoWMIRegistrationControl FAIL");

  status = STATUS_SUCCESS;

  Trace0((PC0C_COMMON_EXTENSION)pDevExt, L"AddFdoPort OK");

clean:

  if (!NT_SUCCESS(status) && pDevExt)
    RemoveFdoPort(pDevExt);

  StrFree(&ntDeviceName);
  StrFree(&portName);

  return status;
}

VOID RemovePdoPort(IN PC0C_PDOPORT_EXTENSION pDevExt)
{
  Trace0((PC0C_COMMON_EXTENSION)pDevExt, L"RemovePdoPort");

  IoDeleteDevice(pDevExt->pDevObj);
}

NTSTATUS AddPdoPort(
    IN PDRIVER_OBJECT pDrvObj,
    IN ULONG num,
    IN BOOLEAN isA,
    IN PC0C_FDOBUS_EXTENSION pBusExt,
    IN PC0C_IO_PORT pIoPortLocal,
    OUT PC0C_PDOPORT_EXTENSION *ppDevExt)
{
  NTSTATUS status;
  UNICODE_STRING portName;
  PDEVICE_OBJECT pNewDevObj;
  UNICODE_STRING ntDeviceName;
  PC0C_PDOPORT_EXTENSION pDevExt = NULL;

  status = STATUS_SUCCESS;

  RtlInitUnicodeString(&portName, NULL);
  StrAppendStr0(&status, &portName, isA ? C0C_PREF_PORT_NAME_A : C0C_PREF_PORT_NAME_B);
  StrAppendNum(&status, &portName, num, 10);

  RtlInitUnicodeString(&ntDeviceName, NULL);
  StrAppendStr0(&status, &ntDeviceName, isA ? C0C_PREF_DEVICE_NAME_A : C0C_PREF_DEVICE_NAME_B);
  StrAppendNum(&status, &ntDeviceName, num, 10);

  if (!NT_SUCCESS(status)) {
    SysLog(pBusExt->pDevObj, status, L"AddPdoPort FAIL");
    goto clean;
  }

  status = IoCreateDevice(pDrvObj,
                          sizeof(*pDevExt),
                          &ntDeviceName,
                          FILE_DEVICE_SERIAL_PORT,
                          FILE_DEVICE_SECURE_OPEN,
                          TRUE,
                          &pNewDevObj);

  if (!NT_SUCCESS(status)) {
    SysLog(pBusExt->pDevObj, status, L"AddPdoPort IoCreateDevice FAIL");
    goto clean;
  }

  HALT_UNLESS(pNewDevObj);
  pDevExt = (pNewDevObj)->DeviceExtension;
  RtlZeroMemory(pDevExt, sizeof(*pDevExt));
  status = InitCommonExt((PC0C_COMMON_EXTENSION)pDevExt, pNewDevObj, C0C_DOTYPE_PP, portName.Buffer);

  if (!NT_SUCCESS(status)) {
    SysLog(pBusExt->pDevObj, status, L"AddPdoPort FAIL");
    goto clean;
  }

  pDevExt->pBusExt = pBusExt;
  pDevExt->pIoPortLocal = pIoPortLocal;

  Trace00((PC0C_COMMON_EXTENSION)pDevExt, L"AddPdoPort OK - ", ntDeviceName.Buffer);

clean:

  if (!NT_SUCCESS(status) && pDevExt) {
    RemovePdoPort(pDevExt);
    pDevExt = NULL;
  }

  StrFree(&ntDeviceName);
  StrFree(&portName);

  *ppDevExt = pDevExt;

  return status;
}

VOID RemoveFdoBus(IN PC0C_FDOBUS_EXTENSION pDevExt)
{
  int i;

  for (i = 0 ; i < 2 ; i++) {
    if (pDevExt->childs[i].pDevExt)
      RemovePdoPort(pDevExt->childs[i].pDevExt);
  }

  if (pDevExt->pLowDevObj)
    IoDetachDevice(pDevExt->pLowDevObj);

  Trace0((PC0C_COMMON_EXTENSION)pDevExt, L"RemoveFdoBus");

  IoDeleteDevice(pDevExt->pDevObj);
}

ULONG AllocPortNum(IN PDRIVER_OBJECT pDrvObj, ULONG num)
{
  PDEVICE_OBJECT pDevObj;
  ULONG numNext;
  PCHAR pBusyMask;
  SIZE_T busyMaskLen;
  ULONG maskNum;
  ULONG mask;

  numNext = 0;

  for (pDevObj = pDrvObj->DeviceObject ; pDevObj ; pDevObj = pDevObj->NextDevice) {
    if (((PC0C_COMMON_EXTENSION)pDevObj->DeviceExtension)->doType == C0C_DOTYPE_FB) {
      ULONG portNum = ((PC0C_FDOBUS_EXTENSION)pDevObj->DeviceExtension)->portNum;

      if (portNum >= numNext)
        numNext = portNum + 1;
    }
  }

  if (num == (ULONG)-1)
    num = 0;

  if (num >= numNext)
    return num;

  busyMaskLen = (numNext + (sizeof(*pBusyMask)*8 - 1))/(sizeof(*pBusyMask)*8);

  pBusyMask = ExAllocatePool(PagedPool, busyMaskLen);

  if (!pBusyMask) {
    SysLog(pDrvObj, STATUS_INSUFFICIENT_RESOURCES, L"AllocPortNum ExAllocatePool FAIL");
    return numNext;
  }

  RtlZeroMemory(pBusyMask, busyMaskLen);

  for (pDevObj = pDrvObj->DeviceObject ; pDevObj ; pDevObj = pDevObj->NextDevice) {
    if (((PC0C_COMMON_EXTENSION)pDevObj->DeviceExtension)->doType == C0C_DOTYPE_FB) {
      ULONG portNum = ((PC0C_FDOBUS_EXTENSION)pDevObj->DeviceExtension)->portNum;

      maskNum = portNum/(sizeof(*pBusyMask)*8);
      mask = 1 << (portNum%(sizeof(*pBusyMask)*8));

      HALT_UNLESS3(maskNum < busyMaskLen, portNum, busyMaskLen, numNext);
      pBusyMask[maskNum] |= mask;
    }
  }

  maskNum = num/(sizeof(*pBusyMask)*8);
  mask = 1 << (num%(sizeof(*pBusyMask)*8));

  if ((pBusyMask[maskNum] & mask) != 0) {
    for (num = 0 ; num < numNext ; num++) {
      maskNum = num/(sizeof(*pBusyMask)*8);
      mask = 1 << (num%(sizeof(*pBusyMask)*8));

      HALT_UNLESS3(maskNum < busyMaskLen, num, busyMaskLen, numNext);
      if ((pBusyMask[maskNum] & mask) == 0)
        break;
    }
  }

  ExFreePool(pBusyMask);

  return num;
}

ULONG GetPortNum(IN PDRIVER_OBJECT pDrvObj, IN PDEVICE_OBJECT pPhDevObj)
{
  ULONG num;
  ULONG numPref;
  HANDLE hKey;
  NTSTATUS status;

  numPref = (ULONG)-1;

  status = IoOpenDeviceRegistryKey(pPhDevObj,
                                   PLUGPLAY_REGKEY_DEVICE,
                                   STANDARD_RIGHTS_READ,
                                   &hKey);

  if (status == STATUS_SUCCESS) {
    UNICODE_STRING keyName;
    PKEY_VALUE_PARTIAL_INFORMATION pInfo;
    ULONG len;

    RtlInitUnicodeString(&keyName, C0C_REGSTR_VAL_PORT_NUM);

    len = sizeof(KEY_VALUE_FULL_INFORMATION) + sizeof(ULONG);

    pInfo = ExAllocatePool(PagedPool, len);

    if (pInfo) {
      status = ZwQueryValueKey(hKey, &keyName, KeyValuePartialInformation, pInfo, len, &len);

      if (NT_SUCCESS(status) && pInfo->DataLength == sizeof(ULONG))
        numPref = *(PULONG)pInfo->Data;

      ExFreePool(pInfo);
    }

    if (numPref == (ULONG)-1)
      num = AllocPortNum(pDrvObj, numPref);
    else
      num = numPref;

    if (num != numPref) {
      status = ZwSetValueKey(hKey, &keyName, 0, REG_DWORD, &num, sizeof(num));

      if (!NT_SUCCESS(status))
        SysLog(pPhDevObj, status, L"ZwSetValueKey(PortName) FAIL");
    }

    ZwClose(hKey);
  } else {
    SysLog(pPhDevObj, status, L"GetPortNum IoOpenDeviceRegistryKey(PLUGPLAY_REGKEY_DEVICE) FAIL");

    num = AllocPortNum(pDrvObj, numPref);
  }

  return num;
}

NTSTATUS AddFdoBus(IN PDRIVER_OBJECT pDrvObj, IN PDEVICE_OBJECT pPhDevObj)
{
  NTSTATUS status = STATUS_SUCCESS;
  UNICODE_STRING portName;
  UNICODE_STRING ntDeviceName;
  PDEVICE_OBJECT pNewDevObj;
  PC0C_FDOBUS_EXTENSION pDevExt = NULL;
  ULONG num;
  int i;

  num = GetPortNum(pDrvObj, pPhDevObj);

  RtlInitUnicodeString(&portName, NULL);
  StrAppendStr0(&status, &portName, C0C_PREF_BUS_NAME);
  StrAppendNum(&status, &portName, num, 10);

  RtlInitUnicodeString(&ntDeviceName, NULL);
  StrAppendStr0(&status, &ntDeviceName, C0C_PREF_NT_DEVICE_NAME);
  StrAppendStr(&status, &ntDeviceName, portName.Buffer, portName.Length);

  if (!NT_SUCCESS(status)) {
    SysLog(pDrvObj, status, L"AddFdoBus FAIL");
    goto clean;
  }

  status = IoCreateDevice(pDrvObj,
                          sizeof(*pDevExt),
                          &ntDeviceName,
                          FILE_DEVICE_BUS_EXTENDER,
                          0,
                          TRUE,
                          &pNewDevObj);

  if (!NT_SUCCESS(status)) {
    SysLog(pDrvObj, status, L"AddFdoBus IoCreateDevice FAIL");
    goto clean;
  }

  HALT_UNLESS(pNewDevObj);
  pDevExt = pNewDevObj->DeviceExtension;
  RtlZeroMemory(pDevExt, sizeof(*pDevExt));
  status = InitCommonExt((PC0C_COMMON_EXTENSION)pDevExt, pNewDevObj, C0C_DOTYPE_FB, portName.Buffer);

  if (!NT_SUCCESS(status)) {
    SysLog(pDrvObj, status, L"AddFdoBus InitCommonExt FAIL");
    goto clean;
  }

  pDevExt->portNum = num;
  pDevExt->pPhDevObj = pPhDevObj;
  pDevExt->pLowDevObj = IoAttachDeviceToDeviceStack(pNewDevObj, pPhDevObj);

  if (!pDevExt->pLowDevObj) {
    status = STATUS_NO_SUCH_DEVICE;
    SysLog(pNewDevObj, status, L"AddFdoBus IoAttachDeviceToDeviceStack FAIL");
    goto clean;
  }

  pNewDevObj->Flags &= ~DO_DEVICE_INITIALIZING;
  KeInitializeSpinLock(&pDevExt->ioLock);

  for (i = 0 ; i < 2 ; i++) {
    PC0C_IO_PORT pIoPort;
    int j;

    pIoPort = &pDevExt->childs[i].ioPort;

    pIoPort->pIoLock = &pDevExt->ioLock;

    for (j = 0 ; j < C0C_QUEUE_SIZE ; j++) {
      InitializeListHead(&pIoPort->irpQueues[j].queue);
      pIoPort->irpQueues[j].pCurrent = NULL;
#if DBG
      pIoPort->irpQueues[j].started = FALSE;
#endif /* DBG */
    }

    pIoPort->pIoPortRemote = &pDevExt->childs[(i + 1) % 2].ioPort;

    status = AddPdoPort(pDrvObj,
                        num,
                        (BOOLEAN)(i ? FALSE : TRUE),
                        pDevExt,
                        pIoPort,
                        &pDevExt->childs[i].pDevExt);

    if (!NT_SUCCESS(status)) {
      SysLog(pNewDevObj, status, L"AddFdoBus AddPdoPort FAIL");
      pDevExt->childs[i].pDevExt = NULL;
      goto clean;
    }
  }

  Trace0((PC0C_COMMON_EXTENSION)pDevExt, L"AddFdoBus OK");

clean:

  if (!NT_SUCCESS(status) && pDevExt)
    RemoveFdoBus(pDevExt);

  StrFree(&ntDeviceName);
  StrFree(&portName);

  return status;
}

NTSTATUS c0cAddDevice(IN PDRIVER_OBJECT pDrvObj, IN PDEVICE_OBJECT pPhDevObj)
{
  NTSTATUS status;
  UNICODE_STRING property;

  status = STATUS_SUCCESS;
  RtlInitUnicodeString(&property, NULL);

  StrAppendDeviceProperty(&status, &property, pPhDevObj, DevicePropertyHardwareID);

  if (NT_SUCCESS(status))
    Trace00(NULL, L"c0cAddDevice for ", property.Buffer);
  else {
    SysLog(pDrvObj, status, L"c0cAddDevice IoGetDeviceProperty FAIL");
    return status;
  }

  if (!_wcsicmp(C0C_PORT_DEVICE_ID, property.Buffer)) {
    StrFree(&property);
    status = AddFdoPort(pDrvObj, pPhDevObj);
  }
  else
  if (!_wcsicmp(C0C_BUS_DEVICE_ID, property.Buffer)) {
    StrFree(&property);
    status = AddFdoBus(pDrvObj, pPhDevObj);
  }
  else {
    StrFree(&property);
    status = STATUS_UNSUCCESSFUL;
    SysLog(pDrvObj, status, L"c0cAddDevice unknown HardwareID");
  }

  return status;
}